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ABSTRACT 


The Naval Postgraduate School in Monterey, California is 
currently working on an ongoing project for research in 
autonomous underwater vehicle (AUV) technology. This pro- 
ject comprises two areas of research. The first area is 
research conducted on the system, NPS II AUV. The second 
area is a computer simulation of the actual system. One 
topic which is vital to both areas is three-dimensional path 
planning. The concept of three-dimensional path planning is 
on the order of magnitude of polynomial time and current 
research in this area is limited. This paper reviews my 
findings and submits an algorithm which finds a best path in 
a three-dimensional environment, while avoiding all known 
polyhedral obstacles. The algorithm's concept is to reduce 
the three-dimensional world to a series of two-dimensional 
representations, allowing the algorithm to use tangential 
lines created from the start to nodes on the polygons lying 
between the start and goal, from nodes on polygons to other 


polygon nodes and finally, from polygon nodes to the goal. 
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I. INTRODUCTION 


A. BACKGROUND 

The last few years, technology has advanced 
significantly to see the use of unmanned vehicles 
substantially increase for land, sea and air applications. 
With regards to military applications, tacticians have used 
this technology to improve and extend the range and 
effectiveness of sensors and weapons. In the recent United 
States, and allied coalition forces’ invasion of Iraq in 
DESERT STORM, both land and sea forces used remotely 
operated planes to perform reconnaissance of enemy forces 
with remarkable success. There are other examples of the 
successful use of remotely operated vehicles. For example, 
the Israeli Air Force successfully used remotely operated 
vehicles to perform reconnaissance and intelligence 
collection of enemy strong points prior to air campaigns 
conducted in 1982, resulting in zero losses for the Israelis 
and substantial damage inflicted on enemy forces. (Floyd 
Sa 

Besides air operations, the concept of remotely operated 
vehicles can be incorporated in naval operations. The 
United States Navy's current maritime strategy calls for its 


forces to be forward deployed as possible. This results in 


US Naval forces operating in or near enemy controlled 
waters. However, since Soviet strategies for both land and 
sea operations call for military operations to be planned 
for and conducted in an in-depth multi-layered unit 
configuration for both offensive and defensive operations, 
the use of a remotely operated tethered vehicle with a 
nearby mother ship acting as the brain is not feasible. 
Therefore, with the US Navy's current strategy coupled with 
the Soviet Union's strategy of multi-layered unit 
configurations, the use of autonomous underwater vehicles 
(AUV) for potential military operations could significantly 
improve the US Navy's operational capabilities. These AUVs 
must possess sufficient onboard sensors as well as control 
units and be able to perform a preplanned mission without 
external control. (Robinson 86) 

Even with the current decline of the Soviet Union as a 
world leader due to its internal conflicts and the decline 
of its satellite countries, the use of AUV technology is 
still a viable option. This became apparent with the events 
and operations of DESERT SHIELD and DESERT STORM in the 
Persian Gulf, especially those dealing with mining of the 
gulf by the Iraqi naval forces. In addition to using an AUV 
in mine warfare, an AUV can also be used in many different 
military operations. Examples of some basic types of 


Operations follow: 


1. reconnaissance (both covert and overt) for 
intelligence gathering 


2. surveillance 

3. mine warfare 

4, terrain mapping 

5. supply and resupply for covert units along the coast 

munitions delivery (or an intelligent torpedo) 

The use of an AUV for these type of operations would 
significantly reduce equipment and personnel to enemy 
capabilities, thus reducing operational costs no matter the 
cost of the AUV. Advances in computer technology, 
particularly in Artificial Intelligence, have made it 
possible to begin AUV development for these type of 
missions. 

However, concept of traveling from A to B without human 
interaction requires that the system act and think as a 
human would. One particular area which offers significant 
challenges is path planning such that the system avoids all 
known obstacles within the environment. In a three- 
dimensional environment, the challenge is even more 
interesting since there exists an infinite amount of 
possible paths extending from point A to point B. This 
concept of three-dimensional path planning is also not 
limited to an AUV system, but can be used on any system 


which must traverse a three-dimensional environment. 


B. AUV CONCEPT 

The basic design of an AUV calls for an unmanned 
submersible vehicle with onboard systems and sub-systems 
that provide power, motion control, navigation, obstacle 
detection and collision avoidance with the capability to 
perform a preplanned mission by controlling and monitoring 
onboard systems without any external input. In addition, 
the AUV must be able to replan its mission in the event it 
encounters unplanned for events. This includes onboard path 
replanning to the mission goal in the event the AUV 
encounters an unplanned-for obstacle (Floyd 91). The Naval 
Postgraduate School (NPS) AUV II models this basic concept 
of AUV development; however, an AUV capable of performing 
the type of missions listed above will need to carry 
additional equipment tailored to perform the specific 


mission in order to accomplish the mission. 


C. PATH PLANNING AND REPLANNING 

The concept behind path planning is to find the shortest 
path or a relatively short path from an initial starting 
point to a goal, avoiding all known obstacles in the 
environment. This concept results in a preplanned mission 
which is then stored onboard on of the memory modules of the 
AUV. This path planning process can be accomplished using 
the AUV control unit, but since this process is a-priori to 


the mission execution, it would be more feasible to do the 


process on another system with more capability, This allows 
for faster results determining a path, multiple path 
planning missions and leaves the AUV for other mission 
executions. However, the replanning concept calls for the 
path planning process to be performed on the AUV. The 
concept of an onboard mission planner further implies that 
the planner must have the capability to access onboard 
stored information of the environmental model. It further 
implies that the replanning process must be in strict 
coordination with not only the obstacle avoidance capability 
on the AUV, but also the module for location determination. 
This thesis will address the path planning and replanning 
process. Work on the obstacle avoidance and associated 
problems has been addressed by another NPS AUV II project 


team member (Floyd 91). 


D. OBJECTIVES 
This thesis will address the following research 


questions: 


l. How to define the typical underwater environment? 


2. Given a known underwater world environment, how to plan 
a route which is the shortest path or a relatively 
short path and avoids all known polyhedrons in the 
environment? 


3. How to incorporate the path planner as a on-board 
replanner? 


E. THESIS ORGANIZATION 

The organization of this thesis is in five chapters. 
Chapter II contains background information concerning the 
NPS AUV II system along with a general summary of other AUV 
projects ongoing as well as research conducted on path 
planning. Chapters III and IV discuss implementation of the 
path planning process. 

The main idea behind any path planning algorithm is two 
fold. First, the planner must determine if there exists any 
obstacles lying between the starting point and the ending or 
goal point. If there are not any obstacles, then the path 
from start to goal is the straight line path from start to 
goal. However, if there is one or more obstacles between 
the start and goal, then the path planner must begin the 
planning process to determine the shortest of a relatively 
short path. 

The organization of this thesis follows this idea. 
Chapter III begins with a discussion of how the three- 
dimensional model is depicted. Afterwards, the rest of the 
chapter is devoted to describing the technique used to 
determine if one or more obstacles lie between the start and 
goal. Chapter IV describes in detail the path planning 
process used to determine a best path from start to goal if 
there is one or more obstacle between them. Finally, 
Chapter V are my conclusions and recommendations for future 


research. 


II. NPS AUV PROJECT AND RELATED WORK 


A. SURVEY OF PREVIOUS WORKS 

As stated before, the primary goal of any AUV research 
is to allow the vehicle to perform a mission without human 
intervention during the execution of the mission. To 
operate autonomously, the vehicle must possess adequate 
intelligence to travel a pre-planned route, but also react 
to unplanned for situations. There are several ongoing AUV 
research projects as well as research for path planning for 
both two-dimensional and three dimensional. 

1. Autonomous Underwater Vehicle Projects 

Within the past decade, research on Autonomous 
Vehicle control has made great strides. There are several 
ongoing projects for AUV development conducted by not only 
academic institutions, but also government agencies as well. 
Texas A&M University is one of the academic 

institutions working on AUV research. Most project designs 
not only incorporate the software or an on-board computer 
controller, but also the specific hardware that the 
controller controls. Texas A&M University has taken a 
different approach in the project development. Their 
approach is to develop a generic computer controller capable 


of successfully performing a fully autonomous long range 


underwater mission. Table I shows the mission requirements. 
While not specifically building the hardware for the 
controller to control, a generic vehicle was considered in 
order to provide design parameters in developing the 


Table I - TEXAS A&M MISSION REQUIREMENTS 


MISSION REQUIREMENTS 


Long Range Capability - 3000 Nautical Miles 


Long Duration - 300 Hours 

Precise Navigation Capability 

Collision Avoidance 

Communication Capability with Mother Ship 


High Reliability and Adaptability 





controller. As can be seen from Table II, the vehicle 


reguirements are unrealistic for any type of testing other 


Table II - TEXAS A&M VEHICLE CHARACTERISTICS 


VEHICLE CHARACTERISTICS 
Length 81 feet 
Diameter 13 feet 
Top Speed 12 knots 
Cruise 10 knots 


Maximum Depth 800 feet 


Range 3000 Nautical Miles 
300 hours @ 10 NM/Hour 


Fuel Load 8.1 tons Diesel 


Computational Power 16 SUN Sparc Stations 





than simulation testing. Texas A&M researchers use a large 
vehicle in the development phase with the idea that the 
generic controller can be adapted for other vehicle designs; 
however, with the use of such a large computer platform in 
the design, one major question is whether the controller can 
be adapted to a platform which does not have 16 SUN Sparc 
Stations for computational power. (TEXAS A&M 90) 

The United States Navy in conjunction with DARPA 
initiated an AUV program in 1988 with the primary purpose of 
showing that AUV systems could successfully perform certain 
Navy mission requirements. Table III shows these mission 
requirements along with some of the vehicle characteristics. 
Unlike the Texas A&M project, DARPA is developing a complete 
system of both hardware and software along with a real-time 
simulation capability of the AUV. And like the Texas A&M 
project, DARPA's AUV is a long range platform and not 
suitable for operations within a small area such as a harbor 
facility. (Pappas et.al. 91) 


Table III - DARPA MISSION AND VEHICLE CHARACTERISTICS 


MISSION AND VEHICLE CHARACTERISTICS 


Mission 

a. Tactical Acoustic System 
b. Mine Search System 

C. Remote Surveillance System 


Vehicle Characteristics 
Weight GO CONS 
Depth 1500 feet 
Speed 10 knots 
Range 240 Nautical Miles 
24 hours (d 10 knots 





The University of New Hampshire has done extensive 
research on AUV technology. There initial project began in 
1978 with the completion of the first Experimental 
Autonomous Vehicle (EAVE) and subsequent successful test of 
autonomously following an underwater pipeline. In 1983 this 
vehicle successfully followed a pre-defined path using 
acoustic transponder navigation. In 1986, the university 
built two new vehicles similar to the original, but with 
more computational power. Current interest in their 
research is to use artificial intelligence techniques to 
develop a knowledge based guidance and control system. 
(Blidberg 90) 

The Institute of Industrial Science at the 
University of Tokyo has developed their own version of an 
AUV. The PTEROA150 was built and successfully tested in 
1989. After successfully testing this prototype, the 
university built the PTEROA250 with greater capabilities as 
well as using neural-net technology for control of the 
vehicle. The Institutes research has shown that a neural 
net controller successfully controls the AUV and can be 
adjusted to allow the vehicle to be used in different 
environmental conditions.  (Ura 90) 

2. Path Planning 
Articles which present the more important aspects of 


path planning are presented. To date, there has been a 


10 


considerable amount of research on path planning in a two- 
dimensional model. However, there is limited amounted of 
research for a three-dimensional model. These articles can 
be classified into two main categories of path planning. 
These categories are graph searching techniques and 
potential field methods. In addition, the majority of 
articles that do deal with a three-dimensional model find 
only paths based on the shortest path and do not consider or 
simplify other constraints such as time allotted for travel, 
energy consumption, type of mission, and type of 
environment. 

M. Sharir has done extended research in the area of 
path planning and obstacle avoidance. Sharir presents 
several algorithms for both two-dimensional and three- 
dimensional environments (Sharir 87). He and J. T. Schwartz 
have also collected a group of algorithms on motion planning 
(Schwartz 88). The algorithms they present are 
predominantly graph searching techniques and their work 
catalogs the algorithms into different classes based on each 
of the algorithms properties. Their work gives a good basis 
for further research; however, as they point out, they 
present no research on algorithms based on a best cost 
property other than shortest euclidean distance since there 
is very little research ongoing. 

In his thesis, Ong describes a heuristic search 


algorithm that finds a viable path in a three-dimensional 


11 


grid coordinate system. Ong uses a graph searching technique 
and the algorithm, as the name implies, uses detailed 
heuristic to prune the solution space by selecting only 
viable successor states for further search. The heuristic 
that are used allow the vehicle to model the human decision- 
making process in determining which viable path is followed. 
Since it uses heuristic extensively, it can be considered as 
an informed search and gives a semi-optimized solution. It 
uses a simplified energy consumption rate of 1.2 times the 
overall length of the path for a path with the start and 
goal lying at different depths. For a path with turns, the 
algorithm adds a constant for the size of the turning angle. 
As the size of the turning angle increases, the larger this 
cost constant becomes. This is a better estimate than the 
estimate than the estimate used for depth change; however, 
it is not accurate (Ong 89) 

Charles Warren uses the potential fields method to 
determine a viable path. In his research, Warren uses 
artificial potential fields in a global path planning vice a 
locally selected region. The idea behind his research is to 
apply potential fields around configuration space (C-space) 
obstacles and use these fields to select a safe path. The 
first step in the process is to map the environment into the 
C-space of the robot. Then artificial fields are placed 
around the obstacles in the environment. In a sea 


environment, many obstacles begin at the ground and extend 


12 


up. Since the sea floor is an obstacle as well as these 
protrusions, the result is one massive obstacle. Warren 
compensates by creating the potential fields from horizontal 
planes sliced through the obstacles. Each of these planes 
forms a forbidden region similar to the two-dimensional 
problem. Since Warren's method uses a global method, a 
chosen path is selected and then modified using the 
potential fields to influence its direction. An advantage 
to using this technique is that the probability of becoming 
trapped by a local minimum is greatly reduced since the path 
planning process uses a known viable path. Warren’s 
research shows a viable and fast path planning process. 
However, it has its limitations. A major limitation is that 
the algorithm must know the global environment prior to the 
process beginning. (Warren 89 and Warren 90) 

Martin Herman introduces a fast three-dimensional, 
collision-free motion planner in his research at the 
National Bureau of Standards. In his planner, Herman 
represents the environment in an Octree structure. Using an 
Octree structure, the environment is initially represented 
as a single primitive geometric structure, usually a cube. 
This cube is the root of a octree. Children of a node are 
created if the parent cubic volume is not homogeneous or 
completely filled with an object or completely empty. If 
the cubic volume is not homogeneous, then the cube is 


subdivided into eight cubes (called octants) with each cube 


13 


becoming a child of the parent. Once the environment is 
represented in an Octree structure, Herman uses one of three 
search algorithms. They are A*, hill climbing, and 
hypothesize and test. The path found using one of these 
algorithms finds a collision-free path, but it is not 
necessarily the shortest path. As Herman states, finding 
the shortest path is computational expensive and a 
relatively short path is adequate for many tasks. One other 
major limitation to this method of path planning is that 
considerable memory is required to store the octree. 

(Herman 89) 

Perez and Wesley have introduced in their research, 
an algorithm that works for both two-dimensional and three- 
dimensional models. In determining a safe shortest path for 
an autonomous vehicle, the authors’ algorithm grows each 
obstacle by some parameter that corresponds to the 
dimensions of the vehicle. With the extra dimensions added 
to each obstacle, the best path is assured to be a safe 
path. The algorithm uses a parametric function to allow for 
vehicles that are not able to change their posture in the 
environment. The algorithm uses the vertices, represented 
in the x, y coordinate system, of the obstacle in the two- 
dimensional environment in finding a shortest path. In the 
three-dimensional environment, the grown obstacles are 
represented in the x, y, z coordinate system. However, a 


shortest path whose node set contains only vertices of these 
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grown obstacles will not necessarily be the shortest path. 
As a result, the authors use additional vertices of a 
constant distance from each other. The resulting path is a 
good approximation of the optimal path. (Perez et.al. 79) 

Kanayama has introduced work on two-dimensional path 
planning for his ongoing research with the Yamabico Mobile 
Robot. In his algorithm, Kanayama proposes that fast 
effective path planning can be accomplished using tangent 
lines. In using technique, possible paths are determined by 
determining the tangent lines between the start point and 
all possible obstacles, between the possible obstacles and 
other obstacles and finally the obstacles and the goal point 
(Kanayama 90). 

There has been a significant amount of research 
conducted in pruning or reducing the search space of the 
visibility graph. Montgomery has introduced several 
techniques that does such. One of more interest is the 
technique of finding the farthest points to the left and 
right with relation to the point being considered for 
expansion. Once these points are found, Montgomery's 
algorithm determines the front edge of the obstacle and 
strips off all vertices that comprise the back edge. He 
argues that by stripping off the back edges, the 
computational requirements are greatly reduced since the 
number of vertices is reduced by roughly a factor of two. 


The algorithm also strips from the search space any 


PS 


obstacles that are occluded from the expansion point by 
another obstacle. (Montgomery et.al. 87) 

As can be seen, there is a considerable amount of 
research on path planning in a three-dimensional 
environment. Each of the above methods are viable and 
efficient as a motion planner. However, these methods only 
use a shortest path (or a relatively shortest path) to 
determine which path is the best path to use. I found no 
research which addressed using some other measure to 


determine a path other than shortest path. 


B. THE NPS AUV PROJECT 

The NPS AUV PROJECT is currently comprised of two 
different areas or research. The first area research is 
computer simulation of the overall project in the 
laboratories at NPS; the second area is experimental 
research conducted on the NPS AUV II, using the NPS swimming 
pool as the world environment. 

l. The NPS AUV II Vehicle 

The current NPS AUV vehicle is called the NPS AUV 

II. Its design is based on development of the earlier and 
smaller prototype, the NPS AUV I (Healey et.al. 89). The 
basic vehicle design of the NPS AUV II has been detailed by 
Good (Good 89). Figure 1 illustrates the basic design of 


the vehicle. 
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TOP VIEW 


Sonar transducers Tunnel thrusters 


SIDE VIEW 





Figure l - NPS II AUV 


The NPS AUV II is a submersible vehicle capable of a 


maximum speed of two knots. It has an overall length of 93 
inches. The main body of the vehicle is made of aluminum 
and the constructed as a box. It has a beam of 16 inches, a 


height of 10 inches and a length of 72 inches. The nose 
cone ìs constructed of fiberglass and is 21 inches long, 
extending the length of the vehicle to 93 inches. The 
vehicle displaces 380 pounds and uses fixed ballast. The 
vehicle has four forward control surfaces, four aft control 


surfaces, four tunnel thrusters and two aft screws each 
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powered by 24 volt DC drive motors, providing 1/8 horsepow- 
er. With the eight control surfaces, the vehicle is highly 
maneuverable and has a turning radius of 3 ship lengths. 
The vehicle's power sources are lead-acid gel batteries 
capable of providing up to two hours of propulsion and 
computational power. 

On board sensors include the following systems: 

1. a navigation system sensor comprised of a flux gate 
compass and directional gyroscope, a vertical gyroscope 
and a three axis rate gyroscope system with transla- 
tional accelerometers. 


2. a paddle-wheel speed sensor installed in the nose. 


3. four sonar transducers installed in the nose. 


The on-board computational power is supplied by a GESPAC 
MPU 30HZ processor with a Motorola 68030 CPU. In addition, 
the system also has two megabyte of RAM and a 68882 math 
coprocessor running at 25 MHZ. The operating system is the 
OS-9 multi-tasking operating system. 

2. Computer Simulation 

The computer simulation portion of the NPS AUV 

PROJECT is currently done on a high resolution graphics 
works-station. The basic simulation model of the NPS AUV II 
was developed by Jurewicz (Jurewicz 91). This model uses 
up-to-date performance coefficients characteristic of the 
NPS AUV II. The basic concept of the simulator is to allow 


project team members to integrate individual modules of the 
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project into the simulator for testing prior to 
implementation on-board the NPS AUV II. The following 


modules have been implemented on the simulator: 


1. A simple locomotion generator for generating a path 
given a set of reference points along a path. This 
generator is restricted to a path in which the 
reference points of the inputted set lying on the same 
horizontal plane. 


2. A three-dimensional guidance control module (Magrino 
91) 


3. An obstacle detection and avoidance module using sonar 
technology. This module has also been successfully 
implemented on the NPS AUV II. (Floyd 91) 


Lg 


III. OBSTACLE REPRESENTATION 


One of the first problems I had to solve was determining 
how to represent each three-dimensional obstacle in the 
environmental model . There were two areas I had to 
consider in deciding how to do the representation. 

The first area was that I wanted my path planner to use 
a tangential method similar to the way Kanayama and Crane 
approach two-dimensional path planning (Kanayama 90 and 
Crane 91). The other area of concern was that I wanted the 
path planner to be compatible with not only the NPS pool 
environment where actual testing of the NPS II AUV is 
conducted but also the Monterey Bay, which will be included 
in the simulator at a later date. 

In addition to considering these two concerns, I also 
made some assumptions of the type of obstacles and the type 
of environment. One such assumption I made concerning the 
obstacles was that all polyhedrons were considered to be 
convex in nature. The assumption I made about the 
environment was that if a path existed between the start and 
the goal, it was not a path that first retreated in the 
opposite direction. A path which first retreats in the 
opposite direction is any path that begins in a direction in 
the x-y plane which is plus or minus 180 degrees different 


from the vector from start to goal in the x-y plane. 
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The bay data was collected in two different resolutions. 
The first resolution covered a large area and used a 
distance between depth points of 200 meters; the second 
resolution, while much smaller gave accuracy with a distance 
of 30 meters between depth points. With the location and 
depth known for each point, an XYZ coordinate system could 
be used in computations. In addition, as with the bay data, 
an XYZ coordinate system could be used for the NPS Pool and 
its environment. 

With these two concerns in mind, I decided to represent 
each obstacle in a triply linked list with each obstacle 
composing the main link, each obstacle-face composing the 
obstacle link and each vertex of the face composing the 
obstacle-face link. In addition, the obstacle list 
comprises first the side faces, followed by the top face if 
needed, and then the bottom face if needed. Also, each 
obstacle face list is a doubly linked list and the vertices 
of the face are placed in a counter-clockwise order. Figure 
2 shows the basic concept in this link list structure. 

The size of the link list can become quite large 
especially for a real world environment such as the Monterey 
Bay. However, the size of this link list can be pruned once 
the user inputs the start point and the goal point. For 
example, once the user inputs these points, the algorithm 
can prune from the list all obstacles which lie behind the 


start goal. 
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Figure 2 - Polyhedron Link List 
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IV. PATH PLANNING 


If the visibility check between the start and goal 
points determines that there are obstacles lying between the 
two points, the path planning process begins. With path 
planning in a two-dimensional environment, the process is 
relatively simple since all paths are on the same plane or 
surface. However, three-dimensional path planning does not 
have the same characteristics. The three-dimensional 
environment has an infinite amount of planes and possible 
paths to consider. As a result, I had to develop a process 
which reduced the search space and the number of possible 
paths to consider to a reasonable amount. In developing the 
algorithm, I considered only obstacles which rise from the 
floor of the environment and are convex. By considering 
only these type of obstacles, I do not have to consider a 
possible path which goes underneath an obstacle. Even 
though, as Sharir states, the problem still approaches 


polynomial time in order of magnitude (Sharir 87). 


A. BASIC CONCEPTS OF THREE DIMENSIONAL PATH PLANNING 
In developing an algorithm to find a best path ina 

three dimensional environment, I decided to extend 

Kanayama’s two-dimensional path planning algorithm. 


Kanayama’s technique develops tangents from the expansion 
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point to each valid two-dimensional polygon in determining a 
best path (Kanayama 90). In addition to using expanding on 
his technique, 1 will also expand on Kanayama's terminology. 
l. Polyhedron Intersection 

In order to solve the problem of determining whether 
or not a polyhedron lies between an expansion point and the 
goal, I used vector calculus and developed a simply ray 
tracing algorithm to determine visibility. A point is 
visible with another point if and only if no polyhedrons 
intersect the vector formed by the two points. This 
process of determining if two points are visible or not is 
not only used at the beginning of the path planning process 
with the start and goal points used, but also each time the 
path planning process expands a path. Therefore, for 
explanatory reasons, I use the term "Expanding Point" and 
its variations in the explanation to denote the first point 
used. Similarly, I use "End Point" to denote the point the 
process is expanding to. 

a. General Description. 

The process initially finds the ray formed by the 
expansion point and the goal. From the ray, the process 
determines the vector. Once this vector is found, the 
process finds the equation of the plane formed by the first 
face of the first polyhedron in the list of polyhedrons, 


determines if there exists an intersection between the 
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expansion-goal ray and the plane, and if one exists, 
determines if the intersection point lies within the 
boundaries of the polyhedron face. If the intersection 
point lies between the boundaries, the process ends 
notifying the calling process that a polyhedron lies between 
the two points. Otherwise, the process continues checking 
each face of each polyhedron, notifying the calling process 
that the two points are visible to each other. Table IV 


depicts the pseudo language I developed for the process. 


Table IV - PSEUDO LANGUAGE FOR INTERSECT POLYHEDRON 


intersect polyhedron(pti, pt2, 3D world) 


for(each polyhedron in 3D world) { 
for(each face on polyhedron) { 
plane = find plane equation(3 points on face); 
intersection pt = find intersection point(ptl, pt2, 
plane); 
if(intersection pt !- NO_INTERSECTION) { 
if(lines intersection(intersection, face, plane) 
return(INTERSECT); 
) 


) 


) 
return(NO INTERSECT): 
) 





b. Directed Ray. 
The process begins by determining the parametric 
eguations for the ray formed using the expansion point and 
the goal. I directed the ray from the expansion point and 


the goal so the eguations take on the form in Eguation l. 
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a (X, xc LC 
VIALE Eq 1 
Ze MEC 


Xx 
Y 
Z 


However, in this case, the direction of the ray does not 
matter and adopted this manner for consistency and 
readability. 
c. Plane Equation. 

In determining the equation of a plane formed by 
a polyhedron face, the process uses 3 points ((X,,y,,24), 
(Xo,YorZ2), (%3/Y3,23)} from the polyhedron face the process 
is checking. The process uses these 3 points to determine 2 
vectors in the form of Equations 2 and 3. 


Ww? (x -x)l c Va MS Eq 2 


V2 í(x-x)I * (y DULL ES Eq 3 
Once these two vectors are found, the process finds the 
normal vector to the plane by determining the dot product of 
the two vectors as seen in Equation 4. 

N-V,-V, Eg 4 
N also takes on the form 

N-nI*nJ*n,K Eg 5 
Using Eguation 5, the eguation of the plane can be found by 
substituting a point on the plane into the eguation. If the 
point (X,,Y,,Z,) is used, the result is Equation 6. 

n (x — x.) 3 mn (yv. sy o'n (a Eg 6 
Eguation 6 eguates to Eguation 7 


nix*ny*nz-D-0 Eg 7 
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where D equates to Equation 8. 
D= nX + Ny) + 2,2, Eq 8 
The source code I developed to find the plane equation is 


shown in Table V and in Appendix C. 


Table V - FIND PLANE EQUATION SOURCE CODE 


equation find plane equation(ptl, pt2, pt3) 
point DCL Beez PE3; 
{ 


point vectorl 2, vectorl 3; 
equation pl_eq; 


UCcpcorm 
pectorl 2. 
Vectori 2. 
Lectorl 3. 
Mectorl 3. 
MEStoOBL.3. 


Deze 
pt2. 
pros 
pues 
pt3. 
PE. 


pt1. 
pude 
ptl. 
pEL. 
pt1. 
ptl- 


NK K NK K 
NK A NK K 


/*cross product is the standard cross product 
equation. Function also is in 3dd.c*/ 
pl eq - cross product(vectorl 2, vectorl 3); 
pied .d — (preg. x NET. 0) * prix) + 
ac uem 0 Pe ptL y)); 
PREU MIME A + (PI eqmzww-D) * ptl.z); 
REGULE 





d. Intersection Point. 
Since the process needs to find the intersection 
point formed by the ray and the plane, the values for x, y, 
and z used in Equation 1 can be substituted for x, y, and z 


in Equation 7. This results in Equation 9. 
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MI A 
ny. “Y y. t VD Eq 9 
miz o Zo” Za E SE 


Finally, Equation 10 results when t is solved. 


n,x,*ny,* ny,*D 


t<  — — —  —  — See 
N, (Xg + Xo) * n(ygQ * y,) * n,(z, + Zz) 


Eq 10 


If we let w and Y take the form of Equations 11 


and 12, then t equates to Equation 13 


w = nX, t nX ND Ed 

y = n,(xg - Ach FE yg YY ze) Eg 12 

t= -* Eq 139 
y 


In equating t to Equation 13, there is a check in 
the value of y. By first computing y and determining if 
y = 0, then the ray is parallel to the plane, no other 
computations are necessary, and the function returns to the 
calling function that there is no intersection for the 
checked plane face. If y != 0, then the process finds the 
value for the parameter w. 

If OS t S 1, then the ray intersects the plane 
between the two points. If t < 0 or t > 1, then the ray 
intersects the plane, but not on the line segment formed by 
the two points. If the process determines there is an 
intersection, the value of t is used in Equation 1 to 
calculate the intersection point and the process returns the 
point to the calling function. Table VI shows the source 


code I developed for the process. The source code is also 
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in Appendix C, which contains the source code of functions 


which deal with the three-dimensional environment, 


Table VI - INTERSECTION POINT SOURCE CODE 


point intersection point(point ptl, point pt2, 
equation plane) 
( 
double y : : 0.0; 


y = (plane.x * (pt2.x - ptl.x) + plane.y * 
PENSE (plane.z * (pt2.2 - ptl.z)); 


me (ly) t NS 
intersection.x = 


O* 
NO INTERSECTION; 


elseí( 
w <= -(plane.x * ptl.x -* plane.y * ptl.y + plane.z * 
ptl.z + plane.d); 
A 


if (0.0 «- t && <= 1.0) { 
intersection. pa: S re2 x =) pt lox) set: 
intersection. pl (pt2.y - ptl.y) * t; 
intersection. EU“ 100pDt2- Z2 ptl.z)-* t; 

] 

else 
intersection.x - NO INTERSECTION; 

} 


return (intersection); 





e. Inside Polyhedron Check. 

If the process determines that there is a valid 
intersection point, it then determines if the point lies 
within the boundaries of the polyhedron. To determine this, 
the process projects a ray from the point to a point outside 


the search space, but lying on the same x y z plane as the 
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polyhedron face. The process then determines the parametric 
equations of this ray. Next, by finding the parametric 
equations for each boundary segment of the polyhedron face, 
we can equate the two sets of parametric equations. Since 
there are two unknowns (both parameters), the process uses 
two of the three equations to simultaneously solve for the 
unknowns. 
For example, if the points of the ray are 

(U,, Vi, W,) and (u;, v;, w;), the parametric equations are 
shown in Equation 14. 

x=u + (u -u)s 


SV, UV VS Eq 14 
Z = W + (w -w)s 


< 


Similarly, if the two end nodes of the boundary of the 
polyhedron face are (a,, b,, cj) and (a;, b,, C,), the results 
are shown in Equation 15. 

a Wc 


b, + (b, - b,)s Eq 15 
Ci “MC aS 


X 
y: 
Z 


If we use the first two equations of Equations 14 and 15, we 
can solve for s and t. Equations 16 and 17 depict the 


results. 


ec AP - v;) (a, — aud eee — 
((a,- a) (v- v) - (5,- b) (u, - u)) 


Es ((D, - v,) (wu) 3% (ae EU ME Eq 17 


(la, - a) (v, TAM — (Cpe a 
If O <s < 1 and Os t S 1 are valid statements, 


then the ray intersects the polyhedron face. This process 
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must be done for each boundary of the face with a counter 
counting the number of times the ray intersects the 
boundary, If the counter is an even number, then the 
intersection point is outside of the boundaries of the face 
and thus the projection from the expansion point to the goal 
does not intersect the polyhedron face. Conversely, if the 
count is odd, the start goal ray intersects the polyhedron 
face. 

An easy implementation of determining if the 
counter is odd or even is to use modulo arithmetic. The 
pseudo language in Table VII depicts this process. The 
Source code can again be found in Appendix C. 


Table VII - PSEUDO LANGUAGE FOR POLYHEDRON INTERSECTION 


if (line intersection count modula 2 -- 1) then 
polyhedron intersection - 


True 
else 

polyhedron intersection - False 
end if 





As mention above, the process checks each face of 
each polyhedron. If there is not an intersection between 
the vector and any of the polyhedron faces, the calling 
process is notified by passing it a non-intersection flag. 
However, if there is an intersection, the process stops 


checking the remaining faces and notifies the calling 


nl: 


process of the intersection so that the path planning 
process can begin. 
2. Plane Types 

The use of the plane is used several times 
throughout the process of finding the best path. There are 
two different types of planes used in finding the best path. 
I refer to one of the two types of planes used as a 
"vertical plane". The other plane is the "horizontal 
plane". I refer to a vertical plane as such because two of 
the three points lie on the same X Z plane. I refer toa 
horizontal plane as such because two of the three points are 
on the same X Y plane and it is perpendicular to the 
vertical plane formed by the same expansion and end points. 
The process of finding the equation for both types are the 
same and uses the same process described earlier in this 
chapter. Appendix C contains the source code which finds 
both of these planes. 

3. Two-Dimensional Polygons 

Two-dimensional polygons are the type of obstacles 
which are used primarily for the path planning process. The 
general concept in building the two-dimensional world, the 
Build two d polygon list function, cycles through each face 
of each polyhedron, finding the intersection points of the 
horizontal plane and the line segments of the polyhedron 


face if one exits. If an intersection does exist, then the 
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process puts the point into a link list for the two- 
dimensional list. Figure 3 depicts the structure of this 
link list and Table VIII shows the pseudo language used to 
develop the list. The source code can be found in Appendix 


B. 


FOLYCON START 


| PO!.YIIHDRON_NUMBIR | | POLYIICDRON NUMBER | 


POLYGON START POI. YCON STAR'' 





Figure 3 - 2D Polygon List 


The Next Polygon entry is a pointer which points at 
the next polygon in the linked list. The Polyhedron Number 
entry is the number of the polyhedron which the algorithm 
used to find the polygon. The Polygon Number entry is the 
number associated to when the polygon was formed. The 
Polygon Start entry is a pointer which points to the first 


node or vertex of the polygon. The nodes are in a doubly 
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linked list with "ccw" depicting counter clockwise and "cw!" 


meaning clockwise in order. 


Table VIII - BUILD TWO-DIMENSIONAL POLYGON 


Build two d polygon list(PolyhedronList, Plane) 
{ 
initialize first polygon; 
current polygon = first_polygon 
for(Each Polyhedron in PolyhedronList) { 
for(Each Face on Polyhedron) { 
for(Each Line Segment of the Face) { 
if((intersection = intersection point(nodel, 
node2, plane) != NO_ INTERSECTION) 
Create _polygon (intersection, 
current polygon); 
] 
] 
remove duplicate node(current polygon); 
initialize next polygon; 
current polygon - next polygon; 


return(polygon list); 





4. Number and Types of Paths 

In the two-dimensional environment, the number of 
possible paths is 2" where n is the number of obstacles in 
the environment. As already stated, I do not consider paths 
which go underneath obstacles. As a result, the number of 
possible paths in the three-dimensional environment is 3”. 
In using Kanayama'/s terminology where a plus sign (+) 
denotes a path extending counter clockwise around an 
obstacle and a minus sign (-) denotes a path extending 


clockwise, I extend it to include an up arrow (T) which 
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denotes a path extending over the obstacle. Table VII shows 
the possible paths for an environment with 1, 2 and 3 
obstacles. 

In analyzing these possible paths in Table IX, one 
can see that general paths with no up arrows indicate that 
the path lies on the same plane as the start and goal. In 
addition, there is one path which extends above all the 
obstacles. Thus, all other paths are a combination of the 
two. In further analysis of Table IX, these paths can be 


generalize to include all cases. These cases are: 


l. The path lies on the same plane as the start and goal. 
2. The path goes over all the obstacles. 


3. The path goes over one or more obstacles before going 
around the remaining one or more obstacles. 


4. The path goes around one or more obstacles before going 
over the remaining one or more obstacles. 


5. The path goes over one or more obstacles, goes around 
one or more obstacles and then goes over the remaining 
one or more obstacles or begins the sequence of around 
and back over the remaining obstacles until the path 
reaches the goal. 


6. The path goes around one or mo e obstacles, goes over 
one or more obstacles and then goes around the 
remaining one or more obstacles or begins the sequence 
of over and back around the remaining obstacles until 
the path reaches the goal. 

I could argue that bullets 3 and 4 are subsets of 
bullets 5 and 6 respectively. However, the concept of 


three-dimensional path planning is a difficult one. 


Therefore, I have broken the types of paths as above, 


ES 


allowing me to explain in a more forthright and detailed 


manner the process I developed to find the best path. 


Table IX - POSSIBLE PATHS FOR POLYHEDRON WORLD 


1 OBSTACLE 2 OBSTACLES 3 OBSTACLES 
(A, B) (A, B, C) 


A+B+ A+B+C+ 
A+B- A+B+C- 
A+B Î A+B+CÎ 
A-B+ A+B-C+ 
A-B- A+B-C- 
A-BT A-B-CT 
ATB+ A+BTC+ 
ATB- A+BTC- 


AE A+BTcT 


A- BICI 
A-B+Ge 
A-B+0M 
A-B-C+ 
A-B-C- 
EE BC || 
AaB Ge 
A-BTc- 
A-BTcT 


ATB+C+ 
ABC 
PE Cl 
ATB-C+ 
ATB-c- 
ATB-cT 
ATBTC+ 
ATBTC- 
ATBTcT 
TOTAL POSSIBLE PATHS 
3 27 
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5.  Tangents 

As with Kanayama's two-dimensional path planning 
algorithm, my process uses tangent lines extended from the 
start point or an expansion point to all valid obstacles. 
However, my algorithm extends these tangent lines to not 
only the sides of the obstacle, but also to the top of the 
obstacle. 

These two basic tangent types the process uses in 
the path planning process can be classified as either 
horizontal or vertical tangents. I further sub-classify the 
horizontal tangent as either a plus tangent or a minus 
tangent. 

a. Plus and Minus Tangents 

I define a tangent to a three-dimensional 
polyhedron to be the ray which is formed by the expansion 
point and an intersection point on an edge of the polyhedron 
such that the ray intersects the polyhedron only at the 
intersection point. For the case where a tangent crosses a 
face of a polyhedron, then the first intersection point is 
the considered the tangent point. As in Kanayama's 
algorithm for the two-dimensional environment, I define a 
plus tangent to be a tangent which if it was extended around 
the obstacle so that it encircled the obstacle, it would 
wrap around the obstacle in a counter-clockwise direction. 


Conversely, a minus tangent is a tangent which if the 
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tangent encircled the obstacle, it would wrap around the 
obstacle in a clockwise direction. Figure 4 shows the 
different plus tangents for a two-dimensional polygon. 


Similarly, Figure 5 shows the different minus tangents. 


Figure 4 - Plus Tangents Figure 5 - Minus Tangents 


Up until now, I have shown the plus and minus 
tangents for a two-dimensional polygon. But as Figures 6 


and 7 show, there are an infinite amount of plus and minus 





Figure 6 - Plus Tangents for Figure 7 - Minus Tangents for 
Polyhedron World Polyhedron World 


tangents when dealing with a three-dimensional environment. 
However, this unmanageable amount can be reduced to a more 


manageable amount determined by the type of path being 
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expanded on and the number of polyhedrons within the 
environment. 

The general idea behind this reduction is to find 
the polygon formed by the intersection of a plane and the 
polyhedron. By using this plane to find all the polygons 
formed by the intersection of the plane and the polyhedrons 
in the environment, the three-dimensional world is reduced 
to a two-dimensional representation. Of course, the number 
of plane slices needed will be different for each of the 
types of paths as listed above. See Figure 8 for a 


graphical representation of this intersection. 





Figure 8 - Plane Polyhedron Intersection 


I use Kanayama's basic algorithm for finding each 
of the type of tangents from a point to a polygon. However, 
I expand on it to find only the closest tangent point to the 


expansion point. This case must be accounted for when the 
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expansion point and one of the sides of the polygon lie on 
the same projected ray. Table X shows the pseudo language 
Kanayama has developed to find the plus tangent (Kanayama 
90). The minus tangent code is the same except that the 
"if" structure which checks the sign and order is evaluated 
with -1 and not 1. The sign function accepts an integer 
which the order function returns and returns either a 1 or - 
1 based on whether or not the function is positive or 
negative. The order function is a modification of the area 
of a triangle equation. The function determines the order, 
either clockwise or counter-clockwise, of the three points. 
For further discussion, see Kanayama'/s development of the 


function (Kanayama 90). 


Table X - PLUS TANGENT PSEUDO LANGUAGE 


plus tangent(pt, polygon A) 
( 
q = initial node(polygon_ A); 
doforever { 
if(sign(order(pt, q, next(q)) == 1) q = next(q); 
elsel 


if(sign(order(pt, q, prev(q D == Da = pe (ar 
else break; 


) 


) 


return(g); 


) 





In determining if the expansion point lies on the 
same project ray as a side of a polygon, the process must 


make two checks. The first check determines if the 
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direction of the ray formed by the expansion point and the 
point found by the plus or minus tangent function is equal 
to the ray formed by the expansion point and the vertex node 
counter-clockwise to the expansion point. Similarly, the 
second check determines if the ray formed by the expansion 
point and the vertex node clockwise to the point is equal to 
the ray formed by the tangent point and expansion node. If 
the rays are equal, then the vertex node which is closest to 
the expansion point is determined and returned. Figure 9 
shows these scenarios. Table XI shows excerpts of the C 
code I developed to accomplish this. Appendix I gives the 
complete source code for both the plus and minus tangent 


functions. 


E% EXPANSION PT 


MINUS TANGENT PLUS TANGENT 





Figure 9 - Direction Heuristic Check 
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Table XI - DETERMINING CLOSEST TANGENT POINT SOURCE CODE 


if(atan2(pl.y - node->pt.y, pl.x - node->pt.x) == 
atan2(pl.y - node->ccw->pt.y, pl.x - node->ccw->pt.x)){ 
if(distance(pl, node->pt) < 
distance(pl, node->ccw->pt)) 
return (node->pt); 
else 
return (node->ccw->pt); 


] 
if(atan2(pl.y - node->pt.y, pl.x - node->pt.x) == 


atan2(pl.y - node->cw-=>pt.y, plix - node->cw->pt.x))f{ 
if(distance(pl, node->pt) < 
distance(pl, node->cw->pt)) 
return (node->pt); 
else 
return (node->cw->pt); 
) 


return (node->pt); 





The case of finding the tangent from one polygon 
to another polygon needs to be handled in a different 
manner. The problem encountered in finding this type of 
tangent happens when the tangent node from the first polygon 
is not the point on the polygon which the process is 
expanding from. Figure 10 depicts this case. 

As can be seen in Figure 10, if the plus tangent 
is found using the expansion point, the tangent line 
(Expansion Point, A) cuts across the first polygon, which 
cannot occur for a valid partial path. The same thing 
occurs when trying to find a minus tangent. As a result, I 
had to develop a process which found the appropriate 
tangents between the polygons (tangent lines (C,B) and (E,D) 


respectively). 
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Figure 10 - Tangents Among 
Polygons 


In finding the two appropriate tangent nodes on 
the polygons, 1 had to consider what type of tangent was 
used in arriving at the expansion point. The idea behind 
the process is then to find the tangent node on the second 
polygon from the expansion point. This tangent node is then 
used in finding the tangent node on the first polygon. This 
process of oscillating from one polygon to the next polygon 
continues as long as the tangent line intersects one of the 
two polygons. Upon finding the two points the process 
returns the two nodes in a link list to the calling 
function. Table XII depicts this process in pseudo language 
for each of the different cases in finding the appropriate 
tangent. Appendix I contains the function "tangent" written 


in the C language, which also has all the cases. 
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Table XII - FINDING APPROPRIATE TANGENTS 
polygonl pt = expansion pt; 
/*expansion pt tangent is minus 

plus tangent to polygon2*/ 
do í 


polygon2 pt plus tangent(polygon2, polygon1 pt); 


polygonl_ pt = plus tangent(polygon1, polygon2 pt); 


jwhile (not(visible(polygon1 pt, polygon2 pt); 
/*expansion pt tangent is minus 

minus tangent to polygon 2*/ 
do { 


polygon2 pt minus tangent(polygon2, polygonl1 pt); 
polygonl pt - plus tangent(polygonl, polygon2 pt); 


jwhile (not(visible(polygon1 pt, polygon2 pt); 
/*expansion pt tangent is plus 

plus tangent to polygon 2*/ 
do ( 


polygon2 pt = plus tangent(polygon2, polygonl pt); 
polygonl pt - minus tangent(polygonl, polygon2 pt); 


jwhile (not(visible(polygon1 pt, polygon2 pt); 
/*expansion pt tangent is minus 

minus tangent to polygon 2*/ 
do ( 


polygon2 pt = minus tangent(polygon2, polygon1 pt); 
polygon1_ pt = minus tangent(polygon1, polygon2 pt); 


jwhile (not(visible(polygon1 pt, polygon2 pt); 





b. Vertical Tangents 
Compared to finding the plus and minus tangents, 
finding the vertical tangents is a relatively simple 


exercise. Since I only am concerned about the top vertical 
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tangent of each polyhedron, 1 refer to this tangent as the 
vertical tangent. Figure 11 depicts the vertical plane, 


the obstacle, and the tangent. 
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Figure ll - Vertical Tangent 


Unlike the functions plus tangent and 
minus tangent, which find a point which forms the tangent, 
the process I developed finds not only that one point which 
forms the tangent, but it also finds the intersection point 
of the vertical plane which is opposite to the side of the 
top polyhedron face from which came the first point. Figure 
12 depicts the scenario. I include both intersection points 
because if the vertical extension of a path is to go over 
only one polyhedron, then the second point will eventually 
be needed. Since the vertical plane is already determined, 
then it is logical to expand the path to the second point 
and not the first. In addition, the process finds all 


vertical points to all polyhedrons lying between the 
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expansion point and the end point. In the event that there 
are more than one obstacle, then I have also included a 
check which determines if points are visible not to the next 
point in the list, but to the other points in the vertical 
list as depicted in Figure 13. This allows me to exclude 
all intersection points not needed in developing the 
extended path. Appendix J contains the source code for 


finding these vertical points. 


13” VERTICAL 
POINT 





Figure 12 - Vertical Points for Figure 13 - Vertical Nodes for 
One Polygon More Than One Polygon 


6. Path Representation 
In designing my algorithm for the path planning 
process, I had to develop a structure which I could 
represent the path. This path structure evolved through 
many iterations until I arrived at the final structure. 
Figure 14 shows the structure and the file 3d tan.h is in 


Appendix L contains the source code defining the structure. 
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Figure 14 - Partial Path List 
The Next path pointer and the Last node pointer in 

the list are self-explanatory. The 3 cost entries are used 
in the search function to identify the shortest path to 
expand the path. The cost entry is the cost from the start 
point traveling along the path to the last node added to the 
goal; the estimated cost entry is the euclidean distance 
from the last point added to the goal; and the total cost is 
the summation of the other two costs. The ray direction 
entry is the direction on the X Y plane of the tangent from 
the second to last node to the last node added to path. 
This entry is used in a direction heuristic, which is used 
to prune possible path entries. The type tangent entry is 


the type of the last tangent formed by the last node and the 
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second-to-last node. The entry "Plane number" is the number 
of the horizontal plane slice used in finding the path 
around obstacles. This entry ensures that the same plane is 
used to expand a path around the obstacles. The able2expand 
entry is a flag used by one of the heuristic to determine if 
the path should be extended or not. The integer forward 
identifies the path as built from the start to goal or from 
the goal to the start.  Polyhed4vert identifies the polygon 
which the path will go around after going over the preceding 
obstacles. The Polyhed4vert PTR is a pointer which points 
to an X Y node structure and is used to expand the path from 
a vertical node around an obstacle and over other obstacles 
(path types 5 and 6). I refer to the information in this 
structure as the header information of the path. 
7. Active Node List and Active Paths Heuristic 

The Active Node List is a link list which identifies 
all those nodes which have been added to the paths in the 
partial path list. Figure 15 shows the structure I use in 
this link list. As with the partial path structure, the 
source code for this structure is found in 3đd_tan.h in 
Appendix L. Along with the X Y Z point, the structure also 
has an entry for the cost of the path from start to this X Y 
Z point and an integer which corresponds to when the path 


was found. 
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ACTIVE NODE 
LIST 





Figure 15 - Active Node List 


This linked list is used in a heuristic which 
determines if a path being expanded from has been visited or 
not. If the point has already been visited, the distance 
entry in the structure is compared with the distance from 
the start to the last node in the expanded path plus the 
distance from the last node to the X Y Z point. If the 
distance in the structure of the link list is less than the 
computed distance, then the new point will not be added to 
the link list as an active node and the path will not extend 
to the this point. If the distance in the list is greater 
than the computed distance, then the new point is added to 
the link list as an active node and the path will be 
extended to the new point. In addition, the partial path 
with which the entry, when path found, eguals the entry in 


the list, then the entry, able2expand, is marked as not 
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extendable. Also the distance and when path found entries 
are updated to reflect the data for this new added node to 
the partial path. If the node has not been visited in the 
path planning process, the node with its corresponding data 
is added to the active node list and the partial path is 
extended to that point. Table XIII depicts the pseudo 
language for this heuristic and the source code is in 


Appendix D. 


Table XIII - CHECK ACTIVE NODE PSEUDO LANGUAGE 


check active node Irst(node lise shortest_path, pt, 
new when path found) 
{ 
for(each active _ node) { 
if(active node->pt == shortest _path->last_ node) { 
if(active node->distance > shortest path->cost + 
distance(shortest path->last_ node, pt) { 
mark old path unexpandable(when path found); 
update active node(new distance, 
new when path found); 


return(EXPAND);  /*EXPAND -- 1*/ 
) 
else return(NOT EXPANDABLE); 


} 
) 
/*node not visited yet*/ 
insert node into node list(node list, pt, cost); 
return(EXPAND); 





8. Visibility 
After the algorithm has found the shortest path to 


extend, the algorithm first determine if the last node added 
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to the path is visible either the goal or start point. This 
case is different from the visibility check used for the 
start and goal points because the last node can be on the 
backside of a polyhedron from the goal and still be visible 
with the goal as long as other polyhedrons lie between the 
last node and the goal. Figure 16 depicts this case. The 
concept of finding the path around the obstacle appears 
relatively simple; however the code is not. Table XIV lists 
the pseudo language I developed to solve this problem. 
Figure 17 identifies the points associated with both 


tangents of the situation shown in Figure 16. 


MINUS 
TANGENT 





Figure 16 - Last Node Goal Figure 17 - Points found for 
Visibility both Tangents 


ST 


Table XIV - PSEUDO LANGUAGE FOR VISIBILITY 


Visibility(expand point, goal point, start, two d list, 
shortest _ path) 
{ 
if(shortest_path forward) 
goal = start; 
else goal = goal_point; 
if(!intersect_polyhedron(expand point, goal)){ 
add goal _to goal _ list; 
return(goal list); 
if(shortest path->type tangent = VERTICAL) 
return(NULL); 
else( 
find polygon last node is on; 
/*finds the plus tangent from goal to polygon 
for(each type of tangent from goal to the polygon) 
if(PLUS TANGENT) 
from polygonwPt*— 
plus tangent(polygon->polygon start, goal); 
else 
from polygon pt - 
minus tangent(polygon->polygon start, goal); 


/*checks for intersection*/ 
if(intersect polyhedron(from polygon pt, goal)) 
return(NULL); 
elsef{ /*two points are visible*/ 
place goal and from polygon pt in goal list 
find from polygon pt on polygon 
if(PLUS TANGENT)( 
cycle counter-clockwise 
add points to goal list until arrive at 
expand point 
) 
else( 
cycle clockwise, add points to goal list 
until arrive at expand point 
) 
if(direction heuristic(shortest path, 
goal list->pt) 
return(goal list); 
} 
} 
deallocate and free memory of goal list 
return (NULL); 
} 
} 
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In the pseudo language for the function visible, I 
refer to a function called "direction heuristic". This 
function uses geometric concepts of polygons and rays to 
eliminate a path from extending incorrectly. If Figure 17 
is used as an example, the "direction heuristic" ensures 
that the nodes associated with the minus tangent are not 
chosen, while the nodes associated with the plus tangent. 
Figure 18 depicts why points F and G would not be chosen 
since point B can extend directly to G. Appendix F contains 


the source code for the "direction heuristic" function. 





Figure 18 - Direction Heuristic 
Characteristics 


The "visibility" function returns to the main 
function the goal list. If the goal list is not NULL, then 
the while loop is exited and the algorithm will add the goal 


list to the shortest path, update the header information of 
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the shortest path, delete all other paths, and terminate. 

If the goal list is NULL, then the algorithm enters the loop 
structure to begin extending the best path chosen to extend. 
Prior to extending the path, the algorithm prints to a file 
or to the screen the paths that are in the partial path list 


and the path chosen as the shortest path to extend. 


B.  THREE-DIMENSIONAL PATH PLANNING 

As stated above, my three-dimensional path planning 
follows along the same concepts as Kanayama's two- 
dimensional algorithm. The basic algorithm begins by 
prompting the user to input the start and goal points. Once 
the user inputs these points, the algorithm checks whether 
or not there are obstacles lying between the two points. 
If there are one or more obstacle lying between the start 
point and goal, the algorithm then finds the two dimensional 
representation of the environment using the horizontal plane 
formed by the start and goal points as discussed earlier in 
this chapter. After finding this two-dimensional 
representation, the algorithm begins to find all the initial 
partial paths. After the algorithm finds these initial 
paths, it searches the path list to find the path to expand 
from after the initial paths are found. Next, the algorithm 
checks to determine if either the goal for a forward type 
path or the start point for a reverse path is visible from 


the last node added to the partial path. If it is, the goal 
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is added to that path, the path is marked as a completed 
path, all partial paths are deleted and associated memory 
deallocated, and the process ends. If the goal or start 
point is not visible, then the path is extended to all valid 
points in the environment and the process begins again with 
searching for a new path to expand. Table XV shows this 


basic algorithm in pseudo language. 


Table XV - PATH PLANNER PSEUDO LANGUAGE 


Three-dimensional path planner() 
{ 
Create 3D world(); 
nputtstartiand: goal); 
if(!intersect polyhedron(start, goal)){ 
build best path(start goal); 
return (best path); 


} 

Two D = build two dimensional_representation(start, 
goal); 

Partial paths - Find initial paths(3D world, Two D, 


start, goal); 


while(visible(find shortest path(partial paths), 
start point, goal))f{ 
expand shortest path(shortest path, 3D world, 
Two D, goal,active_node_list); 
) 
Add goal and mark completed(shortest path); 
Delete partial paths(partial paths); 





l. Initial Paths 
In finding the initial paths, the main algorithm or 
the function "main(void) calls the functions 


"start finding paths". This function accepts the polygon 
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list, the start point and the goal point and returns to the 
main function the initial partial paths. Table XVI shows 
the pseudo language I developed for the function. Appendix 
F contains the source code for this function. 


Table XVI - PSEUDO LANGUAGE FOR START FINDING PATHS 


start finding paths(3d world, polygons, start, goal) 


find vertical and horizontal planes 
polygons = build 24 polygon world(3d.wonilkp 
horizontal) 
for(each polygon in list){ 
pt1 = plus tangent(polygon _nodes, start); 
pt2 = minus tangent(polygon _nodes, start); 
if(direction heuristic from start(ptl, start, goal) ( 
if(visible(pt1, start) 
create new partial path(partial paths); 
add start goal(start, goal, new path) 
update header(new path, goal); 
else 
partial paths - up and over(start point, 
3d wordy pris: 


if(direction heuristic from_start(pt2, start, goal) { 
lf(vrsrbTe(ptl; start» 
create new partial path(partial paths); 
add start goal(start, goal, new path) 
update header(new path, goal); 
else 
partial paths - up and over(start point, 
3d world, pt2); 
end for loop 
partial paths - up and over(start, goal, 3d world); 





The function "start finding paths" begins by finding 
the vertical and horizontal planes formed by the start and 
goal. It then proceeds to find the polygons formed by the 
intersection of this horizontal plane and the polyhedrons in 
the world. After finding this polygon list, the process 


Starts with the first polygon in the list and finds both the 
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plus and minus tangent points with respect to the start 
point as described earlier in this chapter. The process 
then uses a heuristic to determine if the direction of the 
ray formed by the start and goal is within plus or minus 180 
degrees of the ray formed by the start and goal points. If 
this heuristic determines that the ray is valid, then the 
process continues by making additional checks. If the 
heuristic determines that the ray is not valid, the process 
stops checking the plus tangent point and begins the process 
over checking the minus tangent point. 

Once the process determines if the ray is valid, it 
begins to check if the start point is visible with this plus 
tangent point. The process uses the polygon list to 
determine if the two points are visible. If a two points 
are not visible using this two-dimensional polygon list, 
then the two points will not be visible using polyhedron 
list since the polygon list is a subset of the polyhedron 
world. By using this smaller polygon list, the process can 
check for visibility faster than if it used the polyhedron 
ees te. 

In determining that the two points are visible, the 
process begins to find a partial path which corresponds to 
the type of path in which all nodes are on the same plane. 
If the two points are visible then the process adds a new 
partial path to the partial path list. This partial path 


list has the start point as the first point and the plus 
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tangent point as the last node. In addition, the process 
also computes the header data of the partial path. 

If the process determines that the two points are 
not visible, then the process begins the phase of finding 
the type of path which goes over one or more polyhedrons and 
then around (at least one) the rest of the polyhedrons to 
the goal. 

In finding the this path, the process calls the 
function "up and over", passing to the function the partial 
paths pointer, expansion point, start point and the either 
the plus or minus tangent point, depending on which point is 
being checked. The function returns the partial path list 
adding to the list the vertical path as described earlier in 
this chapter. The function up and over and the functions 
this function calls can be found in Appendix J for further 
analysis. 

After each polygon in the two-dimensional list has 
been checked, the "start finding function?) then, finds the 
partial path which travels over all the polyhedrons lying 
between the start and goal. It does so by using the same 
function used above. In using the "up and over" function, 
the calling function passes the same parameters, except the 
goal point is passed instead one of the two tangent points. 

The last step the process determines is to find the 
paths which go around one or more polyhedrons and then over 


(at least one) the remaining polyhedrons. This step is not 


58 


as difficult as it appears. Upon analyzing this step, I 
noticed that this step was the similar to the step of 
finding all the paths which go over one or more polyhedrons 
and around the remaining only in reverse order. As a 
result, to find these type of paths, I developed a sub- 
process which extends from the goal and not the start point. 
The "start finding paths" function calls the 
function "up and over from goal". I developed this function 
along the same lines as "start finding paths". For each 
polygon in the two-dimensional list, the sub-process finds 
both tangents points with respect to the goal, the expansion 
point. After finding the these points, the sub-process 
first determines if the expansion point is visible to the 
plus tangent point. If the points are not visible, the 
"up and over" function is called, adding paths extended from 
the goal. After checking the plus tangent point, the sub- 
process then performs the same operations for the minus 
tangent point. To distinguish these paths from the paths 
extended from the start point, the "forward" integer flag in 
the header information of the path is flagged equal to O 
while this flag is equal to 1 if the path extends from the 
start. Upon returning control back to 
mant findingampaths", the function returns the partial path 
list, which in turn returns the same list back to the main 
function. As with the other vertical function, the 


"up_and over from goal" function is listed in Appendix J. 
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When control of the process returns to the main 
function, all the partial paths which are needed to find the 
best path are included in the list. From this list, the 
process searches for the best path from which to extend 
from. 

2. Extending Partial Paths 

In Extending a partial path, the algorithm enters a 
while loop structure which first searches the partial path 
list to find the best path to extend, secondly determines 
the visibility of the last node of the best path chosen, and 
thirdly, based on the results of checking the visibility, 
extends this path or exits the loop. 

a. Search Technique 

After finding these initial paths, the process 
uses an A* (A Star) search to determine which path to 
extend. When considering the various paths for expansion, 
the A* search uses the function f(n) = g(n) + h(n) where 
g(n) is the actual cost of the path from either the start if 
a forward path or the goal if a reverse path; h(n) is the 
heuristic estimate or the cost/distance from either the last 
node to the goal if the path is a forward path or the last 
node to the start point if the path is a reverse path. In 
the header data of each path g(n) is the "cost" entry; h(n) 
is the "estimated cost" entry; and f(n) is the "total cost" 


entry. The minimum f(n) of all possible paths identifies 
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the shortest partial path and determines which path the 
algorithm will use to extend to the next set of nodes. 

The advantage of using the A* Search is that the 
heuristic measure, h(n) is a lower bound of the actual cost 
of the shortest path. As a result, the algorithm is 
admissible, finding an optimal path if one exists. Once the 
A* Search has determined which path to extend, the algorithm 
begins a process similar to the one used to find the initial 
paths. 

b. Extending the Shortest Path 

Extending the shortest path begins with the 
function "extend path". The main function passes to 
"extend path" the following parameters: start point; goal; 
and pointers to the shortest path, polygon list, polyhedron 
list and the active node list. It returns to the main 
function the pointer to the shortest path. 

Table XVII lists the pseudo language I developed 
to extend the path. As can be seen, the concept behind the 
function is to check the type of tangent entry in the header 
information of the shortest path. If the type tangent is 
not a vertical tangent, then the path is extended on the 
plane slice indicated by the plane number entry in the 
header information. However, if the type tangent entry is 
vertical, then a new plane slice must be used. In addition, 


the function is Appendix F. 
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Table XVII - PSEUDO LANGUAGE FOR EXTEND_PATH 


extend path 
( 
if(shortest path->type tangent != VERTICAL) 
find tangents from inner nodes; 
else( 


add on new plane intersection; 
continue vertical path; 


} 


return(shortest path); 





c. Vertical Extension 

In extending the path in which the tangent to the 
last node of the path is a vertical tangent, the process 
first finds the horizontal plane formed by the last node of 
the path and the goal. Then the process finds the 
intersection of the plane with each polyhedron in the world. 
These new polygons are appended to the list of the existing 
polygons with the plane number entry corresponding to the 
number of horizontal planes used finding polygon 
intersections. 

After finding the polygon intersections for 
the new horizontal plane slice, the process must accomplish 
two tasks. The first task is the find the type of paths 
which correspond to the last two types of paths as referred 
to earlier in the chapter. The second task is to extend the 
path to the one node on the horizontal plane slice found 


directly before beginning this process. 
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To find the paths which begin traversing over 
one or more obstacles, then around one or more obstacles and 
then over at least one obstacle or similarly around, over 
and then around are found from a vertical point. As with 
the paths which correspond to the third and fourth types of 
depending on the direction of the path. If the direction of 
the path is forward, then the type of path found is up, 
around and up; however, if the direction is from the goal, 
the type of path is around, up, and around. Since finding 
these two types of paths are the hardest to find, I left 
them for the last task to accomplish and at the time of 
writing of this paper I have not implemented the process 
into the algorithm which I developed. However, Table XVIII 


shows the process written in pseudo language. 
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Table XVIII - PSEUDO LANGUAGE FOR FUNCTION UP, OVER, UP 


up over up(path) 
{ 
For(each polyhedron) { 
from = find top face(1st polyhedron); 
for(each polyhedron not the ist polyhedron) { 
to = find top face (2nd polyhedron) ; 
for(loop 4 times) /*there are 4 tangents to 
find*/ 
tangent list = tangent(to, from, model, mode2); 
if(check tangent _list(tangent_ list) { 
free tangent list(tangent list) 
continue; 
) 


vertical list = find vertical list(tangent_pti; 


tangent_pt2); 
last node = find last node in vertical list; 
new poly list - build two-d polygon list 
(last node, path's 
last node) 
new path list - duplicate path(path) 
new path list - continue vertical path using 
new poly list 
while(not(visible( 
find shortest path(new paths list), 
new poly list, tangent Pt1) 
new path list - extend path(new path list, 
tangent ptl,new poly list); 
/*tangent ptl is new goal point*/ 
) 
add tangent ptl to shortest path; 
using this process' shortest path 
if(direction heuristic with last node in path 
and last node in vertical path) 
add vertical list to shortest path 
else mark path as not able2expand 
delete all partial paths in new list except 
shortest 
place in locally shortest path list 
free new poly list(new poly list); 
) 


) 

add locally shortest path list to partial path list 
after passed in path and connect links 

return(path); 


) 
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In beginning the second task of the process, the 
process uses these new polygons and finds the one node which 
extends the path along the same ray direction in the header 
information. In doing so, the process scans these new 
polygons checking the polyhedron entry. If this entry 
equals the entry of polyhed4vert, then the process finds the 
two horizontal tangents from the last node to the polygon. 
The process then determines the rays of these two tangents. 
The process adds to the path as the last node the tangent 
point whose tangent direction is in the same general 
direction as the ray direction entry in the header 
information. 

In determining if the two rays are in the 
same general direction, I had to consider the case where the 
first plane slice created a polygon with x and y entries in 
the link list that are different from the x and y entries in 
the link list of the polygon formed by the new plane slice. 
Since the first plane and its associated polygons were used 
to determine the vertical points of the path, the new 
polygon used to determine the tangent point will result in a 
different path if this tangent point and start point are 
used. 

In determining if the point should be added, 
I modified the process to first check the type tangent entry 
in the structure which the pointer polyhed4vert_PTR is 


linked to. The process then executes the correct tangent 
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function (either plus tangent or minus tangent). The next 
check the process does is to determine if the difference in 
ray directions is less than .0001. Instead of using zero, I 
use this value since this algorithm may be used on different 
platforms and the precision of the platforms may be 
different. If the difference is, then the point is added to 
the list. If the difference is not, then the process must 
recalculate the vertical intersection points using the 
vertical plane formed by the start point and this new point. 
In both cases, the header information is updated accordingly 
and the function returns to calling function, extend path, 
the shortest path. The function "continue vertical path" is 
in Appendix J. 
(1) Horizontal Extension 

If the function "extend path" determines that 
the shortest path's type tangent entry does not indicate a 
vertical tangent, then the process begins to find all valid 
tangents on the plane slice indicated by the plane number 
entry in the header information. The function which 
performs this work is "find tangents from inner nodes" and 
the source code is in Appendix F. 

The main concept behind this function is to 
cycle through the polygon list. For those polygons which 
have the same plane number as the shortest path's 


plane number entry, the tangent formed by the polygon being 
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checked and the polygon which the last node of the shortest 
path lies as described earlier in this chapter. 

The process continues on to check whether the 
ray formed by the two tangent points intersects any of the 
polyhedrons in the world. If there is not, then the 
shortest path is duplicated entirely, the two tangent points 
are added to the duplicated path. In addition, all points 
which lie on the same polygon as, are between the shortest 
path's last node and one of the tangent points, and are in 
the direction (either plus or minus) the path is advancing 
are added to the path between these two points. After 
completing the duplication and insertion of the new points, 
the header information is updated and the process inserts 
the new partial path into the partial path list after the 


shortest path, reconnecting the link list accordingly. 


C. FINDINGS AND EXAMPLES 

As I have already indicated, the number of paths grows 
in polynomial time and is dependent not only on the number 
of polyhedrons but also the number of faces in each 
polyhedron and number of nodes in each face. As a result, I 
will limit the number of polyhedrons in the environment to 1 
and 2. In addition, I have included a print function which 
prints out each partial path listing prior to an iteration 
of extending the shortest path. A listing of for each 


example is in Appendix M. 
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l. One Polyhedron World 

For the 1 polyhedron world, I use the polyhedron 
described in Table XIX. In the first example, I use the 
starting point (1, 15, 14) and the goal point (40, 15, 14). 
I use the same depth value (Z value) in the example to show 
that the plane slice does correctly work. Figure 19 depicts 
the scenario with a top down view for this example. 

In generating the initial partial path list, the 
algorithm calculated 3 partial paths. Using the same top 
down view of Figure 19, Figure 20 shows the partial paths 
with nodes of the path labeled. 

The reader should notice that the nodes of the path 
which end at a vertex of the polygon in Figure 20 are nodes 
of the polygon found using the horizontal plane formed by 
the start and goal points. These two paths also represent 
the type of paths in which all nodes of the path lie on the 
same plane. Similarly, the one path in Figure 20 which does 
not have a tangent to the polygon as a node of the path 
represents the path which traverses over all polyhedrons to 
the goal. In addition, the intersection points in Figure 20 
represent the intersection of the vertical plane and the top 


face of the polyhedron. 
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Table XIX - ONE POLYHEDRON WORLD 


SE ACH 1 (O 2 15)5(10 20 15) (10 20 3) (10 7 3) 


en AGE ODO US) «(14 20/215), (14 20.339410 20 3) 
SIDE FACE 3: (14 20 15) (14 7 15) (14 7 3) (14 20 3) 
SIDE.FACE 4: (14.7.15) (10.7 15) (10.7 3) (14 7 3) 
TOP FACE: NULL - 010 20 354 20.3) (14.7 3) 


BOTTOM FACE: (10 7 15) (10 20 15) (14 20 15) (14 7 15) 


«0 15 14) 
e 





Figure 19 - One World Poly- Figure 20 - Initial Partial 
hedron (Top Down View) Paths 


The algorithm chose the path (1 15 14) to (10 20 14) 
to extend using the A* search. In checking the visibility 
of the point with the goal, the algorithm determined that 
the expansion point was visible. As a result the correct 
node(s) of the polygon were added to the path along with the 


goal, the header information of the path updated, all other 
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paths, the polygon list and the polyhedron list are deleted 
from memory and the algorithm terminates with the best path 
as (1 15 14) to (7 20 14) to (14 20 14) to (40 15 14). 

In the second example using the one polyhedron 
world, I used the points (1 13 5) and ( 1 40 14) for the 
start and goal points respectively. From the top down view, 
these initial partial paths look similar to the paths in the 
previous example; however, since the start and goal points 
are not at the same depth, the intersection of the 
polyhedron and the horizontal plane produces a polygon with 
different depth values than the first example as shown in 


Figure 21. 


(012463) ,^ 


Gang... (14 13.67 3) «0 1514) 





Figure 21 - Second Example for 
1 Polyhedron 


In this example, as expected, the shortest path is 
the vertical path. Since the vertical path is directly 
visible to the goal, the algorithm adds the goal to the path 


list and again, it performs all the administrative functions 
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to update the header information, deletes all appropriate 
paths and terminates. 

2. Two Polyhedron World 

For the 2 polyhedron world, I use the same 

polyhedron as in the examples for a one world polyhedron. 
Table XX lists the coordinates of the nodes of the second 
polyhedron. In addition, I use the same start and goal 
points in both examples, similar to the first two examples. 
Figure 22 shows the initial partial paths. As can be seen 
and expected, the number of paths has increased with the 
process including of the vertical paths over the first 
polyhedron. In addition, since the second polyhedron is 
taller than the first polyhedron (z values are smaller, thus 
at a shallower depth), Figure 22 also shows those nodes of a 
path marked with an "x". With respect to the one vertical 
path from start to goal, close observation will show that 
the one node on the edge of the top face closest to the 
second polyhedron was not included in the path. This node 
is not needed since the nodes in between this node are 
visible to each other. As in the first example, the process 
chose the partial path (1 15 14) to (10 20 14) as the path 
to extend. Figure 23 shows only those partial paths which 
are generated from extending the shortest path. Since there 
are obstacles between the last node of the path and the 


goal, the process chooses the partial path (1 15 14) to (10 
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20 14) to (14 20 14) to (20 20 14) for the next iteration of 
extending the shortest path. This partial path is visible 
to the goal so the algorithm as before, performs the 
administrative functions and terminates. 


Table XX - SECOND POLYHEDRAL LISTING 


(20 7 15) (20720 15) (2072 0998 OA 


(2020 15) (24 20 15) (24 20 1) (20 20 1) 


(24 20 15) (21 79155) (2499953502 PENNE 
(287 15) (20999 35) 00 TA 
(20 7 1) (2020 1; On TROA 


(20 7 15) (20 20 15) (24 20 15) (24 7 15) 








(10 17.37 3) (24 16.09 1) 

(20 10.85 1) 
Figure 22 - 2 Polyhedral World Figure 23 - Extending a 
and Initial Partial Paths Shortest Path in a Two 


Polyhedral World 


The second example using a two polyhedral world has 
the points (7 13 7) and (40 15 14) as the start and goal 
points. Figure 24 shows those paths generated from the 


start. As in the last example, the second polyhedral 
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obstacle is taller than the other two polyhedral obstacles. 
As a result, the vertical paths which extend to this 
obstacle will have only one intersecting point on the first 
obstacle the path intersects. Again, the nodes of the path 
are displayed with an "x" at the intersection point. After 
developing these initial paths, the algorithm iterates 
through the process 4 times before the expansion point is 
visible with the goal. Figures 25, 26, 27 and 28 depict the 


process the algorithm used to find the goal. 


(40 15 14) 
o 


Q0 1379 1) (24 18.08 1) 


(20 1065 1) 





Figure 24 - Initial Paths for Figure 25 - First Iteration 
Two Polyhedral World (Example 
2) 
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(1077.60 (147 


(40 15 14) v (40 15 14) 
o : ^N o 





Figure 26 - Second Iteration Figure 27 - Third Iteration 


(0 13.79 1) 





Figure 28 - Fourth Iteration 
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V. RECOMMENDATIONS AND CONCLUSIONS 


The algorithm in which the source code is based on does 
not end up finding the shortest path from a start to goal, 
but a best path for the two points for a point traversing 
the environment. The concept of three-dimensional path 
planning was considerably more difficult then I imagined. 

As a result, I was not able to implement in the source code 

the portion of the algorithm which solves the last two types 
of paths presented in Chapter IV. However, if implemented, 

the shortest path can be found. 

I initially started work on solving/finding the type of 
path which lies on the same plane as the start and goal 
points. In solving each of the other 3 remaining types of 
paths which this thesis addresses, I found myself developing 
again how I solved the previously solved types of paths. 
Some changes were as simple as changing the number of 
parameters passed into the function or the structure used in 
passing the return value back from the called function to 
the calling function. Other changes included major 
modifications. 

On area which of source code which took many iterations 
and time in forethought was the header information structure 
for each path. Every time I proceeded in solving the next 


type of path, I discovered I needed more information or in 


Un 


the case of changing a function's structure not needing 
specific datum. 

This algorithm is only the first step in implementing a 
path planner in the NPS II AUV. In addition to including in 
the source code the portion of the algorithm which solves 
the last two types of paths, other modules will need to be 
added. 

One of the most important to me added is a module which 
insures that the path being extended one more node is a safe 
path for an object larger than a point traversing the world. 
Since the NPS II AUV can be thought of as a cylinder 
traveling through the world, the concept of safe path 
planning is not finding the tangential lines to the 
polyhedrons but finding a cylinder which intersects the 
polyhedron at one point. This concept is not as easy as it 
appears. One particular difficult problem will be in 
dealing with polyhedrons in which the sides are not 
perpendicular to the ground. 

Other modules which should be added after the safe path 
module is added concern implementing the coefficient 
characteristics of the AUV in the path planner as well as 
sea and weather conditions and procedures which will 
aggressively limit the size of the world. The order of 
magnitude for this path planner is driven by not only the 
number of polyhedrons, but also the number of nodes in each 


polyhedron. By reducing the search space, time can be saved 
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in the planning process. With regards to the conditions and 


coefficients, they not only affect the performance of the 


AUV, but they also affect the determination of a best path 


and need to be included. 


717 


LIST OF REFERENCES 


Blidberg, D., Chappell, S., Jalbert, J:., Turner, R., Sedo 
G., Eaton, P., "The EAVE AUV Program at the Marine Systems 
Engineering Laboratory," The 1st Workshop on: Mobile Robots 
for Subsea Environments, International Advanced Robotics 
Programme, pp. 33-42, 1990. 


Floyd, C., Design and Implementation of a Collision 
Avoidance System For the NPS Autonomous Underwater Vehicle 
(AUV II) Utilizing Ultrasonic Sensors, Master's Thesis, 
Naval Postgraduate School, Monterey, CA, September 1991. 


Good, M., Design and Construction of a Second Generation 
Autonomous Underwater Vehicle, Master's Thesis, Naval 
Postgraduate School, Monterey, CA, December 1990. 


Herman, M., "Fast Three-Dimensional, Collision Free Motion 
Planning," Proceedings of the 1989 IEEE International 
Conference on Robotics and Automation, pp. 316-321. 


Jurewicz, T., A Real Time Autonomous Underwater Vehicle 
Dynamic Simulator, Master's Thesis, Naval Postgraduate 
School, Monterey, CA, December 1990. 


Kanayama, Y., Two Dimensional Spatial Reasoning (Spatial 
Planning?), Class Notes, CS4313, Naval Postgraduate School, 
Fall 1990. 


Magrino, C., Three Dimensional Guidance For The NPS 
Autonomous Underwater Vehicle, Master's Thesis, Naval 
Postgraduate School, Monterey, CA, September 1991. 


Ong, S., A Mission Planning Expert System with Three 
Dimensional Path Optimization for the NPS Model 2 Autonomous 
Underwater Vehicle, Master's Thesis, Naval Postgraduate 
School, Monterey, CA, June 1990. 


Pappas, G., Shotts, W., O'Brien, M., and Wyman, W., "The 
DARPA/Navy Unmanned Undersea Vehicle Program," Unmanned 
Systems, pp. 24-30, Spring 1991. 


Robertson, R.C. "National Defence Applications of Autonomous 


Underwater Vehicles", IEEE Journal of Ocean Engineering, vol 
OE-11, No. 4., pp. 462-467, Oct 1986. 


78 


Schartz, J., Sharir, M., "A Survey of Motion Planning and 
Related Geometric Algorithms," Artificial Intelligence, vol 
“no 1-3, pp. 157-169, December 1988. 


Sharir, M., "Efficient Algorithms for Planning Purely 
Translational Collision-free Motion in Two and Three 
Dimensions," Proceedings of the 1987 IEEE international 
Conference on Robotics and Automation, vol 3, pp. 1326-1331. 


Texas A&M University, Information Brief Packet of the AUVC 
Project, 1 November 1990. 


Ura, T., "Development of AUV PTEROA," The 1st Workshop on" 
Mobile Robots for Subsea Environments, International 
Advanced Robotics Programme, pp. 195-200, 1990. 


Warren, C., "A Technique For Autonomous Underwater Vehicle 
Route Planning," IEEE Journal of Oceanic Engineering, vol 
INTO. 3, pp. 199-204, 3 July 1990. 


Warren, C., "Global Path Planning using Artificial 
Potentials Fields," Proceedings of the 1989 IEEE 
International Conference on Robotics and Automation, pp. 
BEI - 32]. 


Wilkinson, P. A Mission Executor For an Autonomous 
Underwater Vehicle, Master's Thesis, Naval Postgraduate 
School, Monterey, CA, September 1991. 


Zyda, M., McGhee, R., Kwak, S., Nordman, D., Rogers, R., and 
Marco, D., "Three-Dimensional Visualization of Mission 
Planning and Control For The NPS Autonomous Underwater 
Vehicle," IEEE Journal of Oceanic Engineering, vol 15, no. 
EE 5. 217-221, 3 July 1990. 


n9 


APPENDIX A 


This appendix contains the source code which is in the file main.c 
#define MAIN 


Hinclude "3d tan.h" 
#include "plot.h" 


pr — — ddd ddd ddr 
MAIN 
ccc oee SS ee odos=-=-=-========..-= == scsi == 222 == SA 
void main() 
( 
int choice, iteration_count = 0; 
equation plane_equation; 
paths *current path - NULL; 
a path *goal path = NULL); 


decision(); 
polyhedron list - create obstacle list(); 


boundry - assign boundry points(); 
printf("NnSTART POINT"); 

start point - input point(start point); 
printft nGOADL” 

goal = input point(goal); 


inside boundries(choice, boundry, start point, goal); 
goal direction — atan2(goal.y - start point.y, 
goal.x - start point. x); 


/* CHECK VISIBILITY BETWEEN START AND GOAL */ 

if (tintersect polyhedron(start point, goal)) { 
printf("The best path to follow is from the start to goal"); 
exit(1); 


) 

/* FIND PARTIAL PATHS FROM THE START TO 1ST SET OF TANGENTS */ 

start path - start finding paths(start path, start 2d, start point, goal, 
boundry, head active nodes); 

print each path(start path, ++iteration count); 


#ifdef DOS 
Pause(); 
#endif 


/* FINDS THE BEST PATH FROM THE INITIAL PARTIAL PATH IN THE PATH LIST */ 


While ((goal_path = visibility(find_shortest_path(start_path), 
goal, start_point, start_2d)) == NULL) { 
if(1 t- iteration count) 
print each path(start path, iteration count++); 
else 
Pterationccountii 
print path shortest(shortest path); 
shortest path - extend path(start point, goal, shortest path, start 2d, 
boundry, head active nodes); 


) 
fprintf(fpt2, "NnNnTHE GOAL HAS BEEN REACHED"); 
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Print each path(start path, ++iteration count); 
print path shortest(shortest path); 
shortest path - goal reached update(goal, shortest path, goal path); 


#ifdef DOS 
if (plot2screen) 
draw shortest path(shortest path); 
Hendif 


print_each path(start path, ++iteration count); 
print path shortest(shortest path); 


#ifdef DOS 
if (plot2screen) [ 
closegraph(); 
) 
#endif 


fclose(fpt2); 


This function allows the user to input a pt into a structure (record) 
and returns that structure to the calling function 


E 

point 

input point(pt) 
point pt; 

[ 
double x coord, yocoord, z coord; 
pEsntf('NnInput x, y, z coordinate: |"); 


scanf("%1f €lf %1f", POINT VALUES); /* macro to assign values to x,y,z */ 
Ds xX Coord, pt.y = y coord, pt.z = z_coord; 
return (pt); 


po 
DECISION 
(fie OR KS So SS ee me ee ee eee puces ee ae Soe 
A 
Int decision() 
[ 
int choice = 0; 
char answer; 
dol 


printf("NnCHOOSE ONENnl. IN POOL 1\n2. IN POOL 2\n3. IN POOL 3\n4. IN 
an”); 
printf("5. IN POOL 5\n"); 
scanf("$d", &choice); 
/* ESTABLISH THE ENVIRONMENT */ 


switch(choice) { 
case l: 
filename - "pool.dat"; 
break; 
case 2: 
filename 
break; 
case 3: 
filename = "pool3.dat"; 
break; 
case 4: 
filename = "pool4.dat"; 


apostI dat"; 
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POOL 


break; 
Case 5: 
filename = "pool2.dat"; 
break; 
default: 
printf("\n\nINCORRECT ENTRY. Try again"); 
elrscr(); 


Jwhile(!tchoice),; 


printf("Do you want to print results to a file (y or n)? "); 
fflush(stdin); 
answer = getc(stdin); 


if (answer == ‘y’ | | answer == 'Y') ( 
print2file = 1; 
printf("\nThe file name is path.doc.Nn"); 
] 
else 
print2file - 0; 
if (print2file) ( 


if ((fpt2 - fopen("path.doc", WRITEONLY)) == NULL) [( 
perror(" Error: Data file for wrinting did not open correctly.\n "); 
exit(0); 
] 
] 
#ifdef DOS 


printf("\nDo you want a plot of this path on the screen (y or n)?"); 
fflush(stdin); 
answer = getc(stdin); 
if (answer == ‘y’ answer == 'Y') 
plot2screen - 1; 
else 
plot2screen 
#endif 


Dc 


printf("NnDo you want a horizontal path only (y or n)? ^"); 
fflush(stdin); 
answer - getc(stdin); 
if(’y’ == answer) 
horizontal = 1; 
else 
horizontal = 0; 
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This function checks whether or not the start point and goal point/points 
are within the boundries of the pool. It calls boundary check passing to 
it the point to be checked.*/ 


void inside boundries(choice, boundry, start point, goal) 
int choice; 
boundries boundry; 
point start point, goal; 
{ 
int boolean = FALSE; 


if (choice 44 (!boundry check(boundry, start point))) [( 
printf("AnThe start point is not within the boundariesin"); 
boolean = TRUE; 


if (choice && (!boundry check(boundry, goal))>) ( 
printf("\nThe goal is not within the boundaries\n"); 
boolean = TRUE; 


if (boolean) 
exit(1); 


This function determines if the passed in point lies within the boundries 
of the search space. If it is not, a FALSE (0) is return, else a TRUE (1) 
is returned to the calling function.*/ 


int boundry check(boundry, pt) 
boundries boundry; 
point pt; 


if (pt.x > boundry.xl && pt.x < boundry.x2 && 
pt.y > boundry.yl && pt.y < boundry.y2 && 
pt.z >= boundry.zl && pt.z < boundry.z2) 
return (TRUE); 

else 
return (FALSE); 
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This function assigns the boundry points to the search area. At present 
it is fixed at the pool dimensions used in the pool.dat; however, at a 
later date, input function can be encorporated to allow the user to speci- 
fy the search area. 


i 


boundries 
assign boundry points() 


[ 


boundries boundry; 


boundry.xl 
boundry.x2 
boundry.yl 
boundry.y2 
boundry.zl 
boundry.z2 


00 => 


cŷn =>. 
me 


O HO WON 
POW Of © 


rim: 


1 


return (boundry); 


This function finds the shortest path and assigns the global pointer to 
it*/ 


point 
find_shortest_path(start_path) 
paths *start_path; 
{ 
/* next for structure is used only to find shortest path */ 
paths *current path; 
point pU 


shortest path - NULL; 


for (current path - start path, shortest path = current path, 
current path t= NULL; 
current path - current path-»^next path) [ 


if ((current path->total cost «€ shortest path-»total cost) 
&& current path->able2expand) 
shortest path - current path; 
if(Ishortest_path->able2expand) 
shortest path - shortest path-»next path; 


if (plot2screen) 
id shortest path for expansion(shortest path); 


return (shortest path-»last node-»^pt); 
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APPENDIX B 


meuelude "3d tan.h” 


po 
BUILD TWO D POLYGON LIST 

ME ES A e e I a a ls 

This function is the major function which builds the two dimensional environment 

from the 3 dimensional environment. It accepts as a parameterthe perpendicular 


plane (labeled plane) and places into a link list theintersection points of a 
polyhedron with the plane. These intersectionpoints form the polygon. 


ZA 
polygon_list *build two d polygon list(start 2d, plane, null ptr) 


polygon list *start 2d; 


equation plane; 
int null ptr; 
[ 
obstacle list *current polyhedron - polyhedron list; 
polyhedron obstacle Scunrrent face; 


polyhedron obstacle plane *current_node; 
polygon_list *polygon, *last polygon, *previous; 


int count intersection count, boolean = FALSE; 
static int polygon number - 1; 
point pto pt2; 
plane_number++; 
if(start_2d != NULL) { 
for(last polygon = start_2d; /*finds last 2d polygon*/ 
last_polygon->next_polygon != NULL; 


previous - last polygon, last polygon - last polygon-»next polygon); 
) 
for (current polyhedron = polyhedron list; current_polyhedron != NULL; 
current polyhedron - current polyhedron->next_ polygon) ( 


if (start_2d == NULL) | 
NEW_POLYGON(start_2d); 
last polygon -» start 2d; 

plane number - 1; 


else ( 
if (null ptr) [ /* create a new node if the last node is not null */ 
NEW POLYGON(last polygon-»next polygon); 
previous - last polygon; 
last polygon - last polygon-»next polygon; 
) 


last polygon-»polygon start - NULL; 

last polygon-»next polygon - NULL; 

last polygon-»plane number - plane number; 

last polygon-»polygon number - polygon number-*-*; 

last polygon-»polyhedron number -» current polyhedron-»polyhedron number; 


for (current face - current polyhedron-»obstacle, intersection count - 0; 
currenteFuace to NULL; 
current_face = current face-»next face) [ 


for (current node - current face-»face nodes, count = l, 


boolean = FALSE; 
count <= current_face->face nodes-»number nodes; 
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current node - current node->ccW, countrr) { 


if (boolean -- FALSE) [ 
ptl - intersection point(current node-»pt, current node-»ccw-»pt, 
plane); 
if (ptl.x !- NO INTERSECTION) [ 


boolean = TRUE; 
intersection count++; 
break; 
] 
J 


) 
if (boolean) { 
last_polygon->polygon_start = create_list(last_polygon->polygon_start, 
intersection_count, ptl); 
] 
] 
if (last polygon-»polygon start !- NULL) [ 
null ptr = TRUE; 
last_polygon->polygon_start->number_nodes = 
count polygon nodes(last polygon); 
last polygon -» remove duplicate nodes(last polygon); 
last polygon - connect links(last polygon); 
] 
else 
null ptr = FALSE; 


] 

if(last polygon-»5polygon start -- NULL) 
previous-»next polygon = NULL; 

return (start 2d); 


This function accepts the polygon start, the intersection count, and the ptl 
parameters and places the point (ptl) into the link list for 2d polygons 
(polygon start). 

"A 


obstacle plane *create list(polygon start, intersection count, ptl) 
obstacle plane *polygon start; 
int intersection count; 
point pul 


obstacle plane *temp ptr, *current, *previous; 
int count; 
obstacle plane *two d ptr; 


if (polygon_start == NULL) ( 
NEW PLANE(polygon start); 
polygon start-»pt - assign point values(ptl); 
polygon start-»number nodes - intersection count; 
polygon start-»ccw - polygon start-»cw - NULL; 
) 
else [ 
NEW PLANE(temp ptr); 
temp ptr-»pt - assign point values(ptl); 
temp ptr-»ccw - NULL; 


for (previous = polygon start, current = polygon start, count = l; 


current != NULL; 
previous = current, current = current->ccw, count++) ( 
if (current->pt.x == ptl.x ££ current->pt.y == ptl.y 4£ 
current->pt.z == ptl.z) 1 
intersection count -= 1; 
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polygon start-»number nodes - intersection count; 
free(temp ptr); 
return (polygon start); 
] 
] 
previous->ccw = temp ptr; 
polygon_start->number_nodes = intersection_count; 


return (polygon start); 


This function assigns the coordinate values (x, y, Z) to ptl which is declared as 
a point structure 


p^ 
point assign point values(pt2) 
point pt2; 
[ 
point pel; 
pel.x = pt2.x; 
ptl.y = pt2.y; 
Ber.z = pt2.z; 


return (ptl); 


This function connects the links of the polygon list so that each polygon is can 
be accessed using a doublely linked list 


ar 


polygon list *connect_links(polygon) 
polygon_list *polygon; 
( 


obstacle plane *current - polygon->polygon start; 


int count; 
for (; current->ccw l= polygon->polygon start; current = current->ccw) 
current->ccw->cw - current; 


polygon->polygon_start->cw = current; 
return (polygon); 


This function counts the number of nodes that make up the polygon and places the 
value in the the polygon's start node structure 


WA 


int count polygon nodes(polygon) 
polygon list *polygon; 
[ 


obstacle plane *current = polygon->polygon_start->ccw; 
Put count = 1; 
for (; current t= polygon->polygon_start; 
current = current->ccw, counttt) 
if (current->ccw == NULL) 
current->ccw - polygon->polygon start; 
return (count); 
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This function removes any duplicate nodes which are placed in the polygon link 
listed when created 

AE 

polygon list *remove duplicate nodes(polygon) 


polygon list *polygon; 


obstacle plane *start - polygon->polygon start, 
*current = start->ccw, *previous = start; 
int countl, number duplicate nodes - 0; 


for (countl = 1; 
countl1 <= start->number_nodes; 


countl++, previous = current, current = current->ccw) |[ 
if (current->pt.x == previous->pt.x && 
current->pt.y == previous->pt.y && 
current->pt.z == previous->pt.z) f 


number duplicate_nodestt; 


if (start->pt.x == current->pt.x ££ 
start->pt.y == current->pt.y && 
start >pt:z —— current >pt zZ) | 
polygon->polygon start - previous; 
previous-»number nodes - current-»number nodes; 


) 

previous->ccw = current->ccw; 

free((char *) current); 

current = previous->ccw; 

) 

] 
polygon-»polygon start-»number nodes -- number duplicate nodes; 
return (polygon); 


This function deallocates the memory associated with the polygons formed by the 
intersection of the perpendicular plane and the polyhedron 


x 


polygon list *remove planes(start 2d) 
polygon list *start 2d; 
[ 


polygon list *previous — start_2d, *current = start_2d->next_polygon; 
obstacle_plane *nodel, *node2; 


for(; current-»next polygon !- NULL; 
previous = current, current - current->next polygon)[ 
for(nodel - previous->polygon_ start, node2 - nodel->ccw; 
node2 t= previous->polygon start; 
nodel = node2, node2 = node2->ccw) 


free(nodel); 
free(nodel); 


free(previous); 


) 
return(NULL); 


) 
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APPENDIX C 


miunclude "3d tan.h" 


This function is the main function used to create the 3D obstacle environ-ment. The 
list of obstacles use pointers with the main list made up of major obstacles only. 
Off each record in this list a pointer points to the first face of the polygon, 
which in turn points at the next one. This continues until a list of faces is 
established. With in each face record is a pointer which points at a linked list 
of nodes. It calls create face passing to it the pointer within the polygon record 
that points to the first face of the polygon. It also passes the file pointer to 
the data file*/ 


Blstacle list  *create obstacle list(void) 
[ 
FILE *fpt; 
int count; 
obstacle_list *polyhedron_list, *polygon = NULL; 


/* opens file if one, else exits program on an error */ 
if ((fpt = fopen(filename, READONLY)) == NULL) ( 
perror(" Error: Data file did not open correctly.\n "); exit(1); 


else { 
read comment (fpt); 
fscanf(fpt, "3d", ¿number polygons); 
for (count = 1; count <= number polygons; count++) ( 
if (count == 1) { 
NEW NODE(polyhedron list); 
polygon - polyhedron list; 


else ( 
NEW NODE(polygon->next polygon); 
polygon - polygon->next polygon; 


polygon->polyhedron number - count; 

polygon->obstacle - create face(polygon->obstacle, fpt); 
polygon->next polygon - NULL; 

] 


] 
return (polyhedron list); 


This function creates the list off the major polygon list and makes up a list of 
polygon faces. It call create plane, passing it the file pointer to the data file 
and the pointer within each face that will point to the first node of the linked 
node list. It returns a the start record (first face in the polygon list) to the 
calling function*/ 


polyhedron obstacle *create face(start face, fpt) 
polyhedron obstacle *start face; 
FILE *fpt; 


int number_faces, count; 
polyhedron_obstacle *face; 
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fscanf(fpt, "$d", &number faces); 

for (count = 1; count «- number faces; counttt*) { 
if (count == 
NEW POLYHEDRON(start face); 
face = start_face; 


) 


else [ 
NEW POLYHEDRON(face-»next face); 
face face nc UE cc, 


] 
face-»next face - NULL; 
face->face nodes - create plane(fpt, face-»face nodes); 


] 


return (start face); 


This function creates the floatly linked list made up of the nodes of the face and 
returns the start of the list to the calling function.*/ 


polyhedron obstacle plane *create plane(fpt, start plane) 
polyhedron obstacle plane *start plane; 


FILE *fpt; 
[ 
polyhedron obstacle plane *current - start plane, *previous = NULL; 
int count = 1l, nodes; 
double X, Y; Z; 


previous = NULL; 
fscanf(fpt, "%d", &nodes); 


for (count = 1; count <= nodes; count++) [ 
fscant(fpt, "If tli Sli", Ex, Ey, EZ), 
if (count == 1) { 
NEW_POLY PLANE(start plane); 
previous = start plane; 
current = start plane; 
else [ 


NEW_POLY PLANE(current->ccw); 
current = current->ccw; 


) 


current->pt.x = x; 
eurrentz>pt. Y =y. 
current->pt.Z2 — Z; 
current->cw = previous; 
current->ccw = NULL; 
previous = current; 


start_plane->cw = current; 
current->ccw = start plane; 

start plane-»number nodes - nodes; 
return (start plane); 


This function is used only to strip the comments off the polygon data file so that 
they do not corrupt the link list, but an explanation of the file makeup remains 
with the data file 


a7, 


void read comment(fpt) 
FILE * IDE. 
[ 
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char word[20]; 


do 
fscanf(fpt, "ss", word); 
while (word{2] t= '$8'); 


This function calculates the distance from one point to another and re-turns it to 
the calling function*/ 


double distance(ptl, pt2) 
point pel, DeZ; 
[ 
double a, D C, Z; 
a= ptl.x - pt2.x; 
a  sqr(a); 
bo ptl.y - pt2.y; 
b = sqr(b); 
EET ptl.z - pt2.z; 
cuv sgQr(c); 
Z - sqrt(a * b * c); 


return (2); 


/*------------------------------------------------------------------------------ 
FIND PLANE EQUATION 

eo rr hh A aala OUCGLLL. a O e e e e em. 

This function finds a plane give three points. It finds two vectors formed by 


ptlpt2 and ptlpt3, then calls cross product to find the a, b and c coefficents, and 
finally solves for d using a,b,c and ptl */ 


equation find plane equation(ptl, pt2, pt3) 
point ptl, pt2, pt3; 

[ 
point vectorl 2, vectorl 3; 
equation pl eq; 
EGCLOrl|- 2.x - pts uptl.*x; 
vectorl 2.y = pt2.y - ptl.y; 
Eeostobrha2 2 > pt2:2 =. ptilsz; 
Vector 3.x = pt3.x - ptl.x; 
vectorl 3.y = pt3.y - ptl.y; 
Yu 3.2 = pt3.2 > ptl.z, 


pl eq -» cross product(vectorl 2, vectorl 3); 

pil eqd= ((pheg x * (-1.0) * ptl.x) + (pl eq.y * (-1.0) * ptl.y)); 
edad please (EL 0) * ptl.z); 

return (pl eq); 


This function finds the cross product of two vectors and returns the coefficients 
to the calling function.*/ 


equation cross product(vectorl, vector2) 
point vectorl, vector2; 
[ 
equation plane; 
plane.x = (vectorl.y * vector2.z) - (vector].z * vector2.y); 
plane.y - -1.0 * ((vectorl.x * vector2.2) - (vectorl.z * vector2.x)); 


SET 


plane.z - (vectorl.X«* vVector2.y) - (vectori r ivector nX): 


return (plane); 


This function determines if the line formed from two points intersect a polyhedron. 
If so, it returns l, else it returns zero. It calls find_plane equation, passing 
it three nodes on the current face being checked and receives the plane equation. 
It then calls inter-section_point to đetermine if the line intersects the plane. 
This function returns the point if there is an intersection, else it sets the x 
coeffi-cient = -999999, If there is an intersection point, the function call 


lines intersect, which determines if the intersection is within the boun-dries of 
the plane. 


A 


int intersect polyhedron(ptl, pt2) 
point pti; pt2; 


obstacle_list *current_obstacle; 
polyhedron_obstacle *face; 
polyhedron obstacle plane *node; 
equation pl eq; 


for (current obstacle - polyhedron list; current obstacle !- NULL; 
current obstacle - current obstacle-»next polygon) ( 


for (face e current obstacle->obstacle; face t= NULL; 
face = face->next face) | 


node = face->face nodes; 


pl_êg -> find plane eguation(node-?>pt, node->cew pt; 
node->ccw->ccw->pt); 


if ecd * ptl.x * pl eq.y * ptl.y + pl_eq.z * ptl.z + pl eq.d) ==10 


(pl_eq.x * pt2.x + pl_eq.y * pt2.y + pl eg.2 * pt2.z t p ceg dE RE 


continue; /* if ptl lies on the plane, go to next plane */ 
intersection = intersection_point(ptl, pt2, pl eq); 
if (intersection.x !- NO INTERSECTION) { 


if (!lines intersect(intersection, face-»face nodes, pl eq)) 
return (1); 


) 
] 


return (0); 


- - DA E E E E I M a «- - a « = = = 


This function finds the intersection of a plane and a line. 


It accepts three 
arguments, 


2 points (the line) and an eguation for the plane. It returns the 
intersection point if there is one or it sets the plane.x value egual to -99999.0. 
This value is a flag that lets the calling function know that there is no 
intersection point and the line and plane are parallel.*/ 


point intersection point(ptl, pt2, plane) 
point ptl, BL 
eguation plane; 
double yD O ab OD 
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“lane ptl.x) + plane.y * (pt2.y - ptl.y)); 
y (plane.z *Spt27WE- ptl.z)); 

f (y == FALSE) ( 

intersection.x = NO_INTERSECTION; 

return (intersection) ; 


beh A 


plane. pti x + plane.y * ptl.y + plane.z * ptl.z + plane.d); 
p -1*t/y; 
if (0.0 <= t && t <= 1.0) f 
intersection.x = ptl.x + (pt2.x - ptl.x) * t; 
intersection.y = ptl.y + (pt2.y - ptl.y) * t; 
Mhtersection.z = ptl.z + (pt2.z - ptl.z) * t; 
) 
else 


intersection.x = NO_INTERSECTION; 


return (intersection); 


This function determines if a point is within the boundries of a polygon. If the 
point is, the function returns 1, else it returns 0. Since the plane can be on any 
plane in the 3d environment, the function must check each plane face (Xy, XZ, yZ). 


EZ 


int lines intersect(intersect, start, pl eq) 
point intersect; 
polyhedron obstacle plane *start; 
equation pl eq; 


polyhedron obstacle plane *current - start; 


int number nodes - start-»5number nodes, count; 
int intersect count - 0; 

double t s, DIC; 

point temp_pt; 


temp_pt = find_point_on_obstacle_plane_face(intersect, pl_eq); 
for (count = 1; count <= number nodes; countt+, current = current->ccw) [ 


/* CHECK X Y PLANE */ 


bo- (tempopt.xweMintersect.x) * (currents»ccw-»pb.y - current-?pt.y); 
c = (temp pt.y - intersect.y) * (current->ccw->pt.x - current->pt.x); 
D =D = C; 


if (b != 0) [ 


s = (intersect.y - current->pt.y) * (current->ccw->pt.x - current->pt.x); 
s -= (intersect. x > current->pt.x) * (current->ccw->pt.y - current->pt.y); 
s =s / b; 

te=- (intersect: y  cürrent >pt.y) * (temp pt.x - inmtersect.X); 

t=U - (intersect.x - current->pt.x) * (temp pt.y,- intersect.y); 

te= t TD: 


if (0 <= s && s <= ] && 0 <= t && t <= 1) ( 
intersect_count++; 
continue; 
] 
) 


/* CHECK X Z PLANE */ 


b= (tenp pryk a tersectay) * (CUErrents>ccw->pt.Zz - current->pt.Z); 
e= (tenp tersectaz) * (currents>ccw->pt.y - current->pt.y); 
B= bD =c; 
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i£ (b 1= 0% 


s = (intersect.z - current->pt.Z) *“(eurrent >cew->pt: y  (CUFEEnE PI 
s -= (intersect.y - current->pt.y) * (Current >ccw->pt.2 (current >pL<«2Z''— 
s = s / b; 

t = (intersect.z - current->pt.z) * (temp_pt.y - intersect.y); 

t=t - (intersect.y - current->pt.y) * (temp pt.z - intersect.z); 
t=t/D 


if (0 <= s && s <= 1 && 0 <= t && t <= 1) ( 
intersect_count++; 
continue; 
) 
} 


* CHECK Y Z PLANE */ 
(temp pt.x - intersSect.x) * (current-»ccw-2pL-z - cHPPentep 
(temp pt.z - intersect.z) * (current-—>ccw >pt x current EU 


7 
b 
c 


if (b - c >= le-3) { 


s = (intersect.z - current->pt.z) * (current->ccw->pt.x - current->pt.x); 
s -= (intersect.x - current->pt.x) * (current->ccw->pt.z - current->pt.z); 
S= s / (b 

t = (intersect.z - current->pt.Z) * (temp pt.x - intersect.x); 

t=t - (intersect.x - current->pt.x) * (temp_pt.z - intersect.z); 

t cO (DES COE 


if (0 <= s && s <= 1 && 0 <= t 44 t <= 1) ( 
intersect count. |, 
continue; 
) 
] 
) 
if (intersect count a 2A SL 
return (17; 
else 
return (0); 


This function finds a point that is on the plane. It is called by the function 


that checks if the intersection of the ray formed by two points intersects any 
polyhedrons in the environment. */ 


point find point on obstacle plane face(intersect, plane) 
point intersect; 
equation plane; 
[ 
point pt; 
pt.z 9-5; 
if ((plane.x -- || plane.y == 0) 44 plane.z == 0) ( 


pt.x = intersect.x; 


Peay intersect.y; 
else { 
pt.y = 17; 
if (plane.x != 0) 
pt.x = -(plane.d + plane.z * pt.z + plane.y * pt.y) / plane.x; 
else 
pt.x = 50; 


) 
return (pE; 
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This function finds the vertical and perpendicular planes formed by two points. 


i^ 


void vertical n perpendicular plane(ptl, pt2) 
point pti pt2; 
point pt3; 
pt3.x = pt2.x; 
D y = pt2.y; 
PEIZ = pE2.Z + 1; 


vertical = find_plane_equation(ptl, pt2, pt3); 
if (ptl. x l= pt2.x) | 


pt3.x = pt2.x; 

PEs. y = pt2.y +t 1; 
else ( 

DEI = DEZA 1: 

pt3.y — pt2.y; 


RLI. Z *-"pt2.2; 
perpendicular = find_plane_equation(ptl, pt2, pt3); 
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APPENDIX D 


finclude "3d_ tan.h" 


This function checks the active node list to determine if the expanded 
node found in find tangents_from inner nodes has already been used as a 
node to an existing path. If the point has been used, the function deter- 
mines if the possible new path has a cost less than the existing path. If 
the cost is less, the function calls the function mark old path with node 
unexpandable, which marks the path as unexpandable. If the new possible 
path has a smaller cost, then the function returns a one (1) else it 
returns a zero (0). 


a 
int check active node list(head active nodes, shortest, pt) 
active nodes *head active nodes; 
paths *shortest; 
point pts 
[ 
active nodes *current - head active nodes; 
a path *last node -» shortest-»last node; 
for (; current !- NULL; current - current-»next active node) ( 


if (current->pt.x == pt.x && current->pt.y == pt.y && current->pt.z — PBL 


if (current->distance > (shortest->cost + 
distance(shortest->last node->pt, pt))) ( 
start_path = 
mark old path with node unexpandable(current-»when path found); 
return (1); 
] 
else 
return(0); 
) 


] 


return(1); 


This functions is called if the existing path has been determined as too 
expensive (ie; there is a node in the path that can be reached by another 
path with a smaller cost). The function searches the path list to find 
the correct path and then marks the flag "able2expand" as FALSE (0). 

a 


paths *mark old path with node unexpandable(path number) 
Tnt path number; 

[ 
paths xcurrent = start_path; 


for (; current !- NULL; current - current-»next path) [ 
if (current-»when path found -- path number) 
current-»able2expand = FALSE; 


) 
return (start path); 
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wn nn nn ee = O000-------------------------------*/ 
active_nodes *insert nodes into active node list(head node list, pt, 
cost) 
active nodes *head node list; 
point pt; 
double cost; 
[ 
static active nodes *last node added - NULL; 
if (head node list -- NULL) [ 
NEW ACTIVE NODE(head node list); 
last node added - head node list; 
last node added-»next active node - NULL; 


] 

else [ 
NEW ACTIVE NODE(last node added-»next active node); 
last node added - last node added-»next active node; 
last node added-»next active node - NULL; 

) 

last_node added->pt = assign point values(pt); 

last_node added->distance = cost; 

last node added-»when path found - number paths found; 

return (head node list; 
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APPENDIX E 


This appendix contains the source code 1 began to implement for going up, around 
and up type of paths as described in Chapter IV. The file over&@.c contains the 
source code developed todate and not yet implemented 


finclude "3d _tan.h" 
include "plotohs 


define OVER 0 


paths *up around up(path, tan, start, goal) 
paths *path; 
int tan; 
Ls 
polyhedron face *from face - NULL, *to face - NULL; 
obstacle list *polyhedronl, *polyhedron2; 
a path *tangent list; 
vertical path *vertical list; 
int count, 
polygon_list *polygons, *recursive_polygons = NULL; 
paths *partial path - NULL, *shortest, *short list; 


for(polyhedronl - polyhedron list; 
polyhedronl t= NULL; 
polyhedronl = polyhedronl->next_polygon) { 
from face - find polyhedron face(polyhedronl); 


for(polyhedron2 = polyhedron list; 
polyhedron2 t= NULL; 
polyhedron2 = polyhedron2->next_ polygon) { 


if(polyhedron2->polyhedron number == polyhedronl->polyhedron_number || 
polyhedron2->polyhedron number -- path->polygon4vert) 
continue; 

else 


to_face - find polyhedron face(polyhedron2); 


/*since tangent returns a tangent between two polygons, have to form a 
polygon list with only the to_ face and from face in the list so that a 
the tangent is found*/ 
polygons - make list(from face, to face); 
for(count = l; count <= 4; count 
if(NULL -- (tangent list - tangent(path, from face, to face-»face nodes, 
(count ==1||3?MINUS:PLUS), polygons, OVER, 
count E 2?MINUS:PLUS))) printf("you blew it caddell"); 


if(t!chech tangent list(tangent list, path)) continue; 


if(tintersect_polyhedron(tangent_list->pt, tangent list-»next node-»pt)) 
continue; 
else[ 


vertical list = find vertical nodes(tangent list-»next node-»pt, 
tangent list->pt); 


vertical_n_perpendicular_plane(vertical_list->next_node->next_node->pt, 
path->last_node->pt); 


recursive polygons = build two d polygon list(recursive polygons, 
perpendicular, FALSE); 


partial paths - duplicate path(path); 
partial paths-»plane number - plane number; 
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partial paths = continue vertical path(partial paths, recursive polygons 


recursive polygons = free polygon_list(recursive polygons); 
] 
]/*end count for structure*/ 
) /*end inner for structure*/ 
). /*end outer for structure*/ 


free pseudo polygon list(polygons); 
free tangent list(tangent list); 

i^ 
return(path); 

} 


polyhedron_face *find_polyhedron_face(polyhedron) 
obstacle list *polyhedron; 


polyhedron face *face, *min_vert_face = NULL; 
obstacle plane *node, *min_vert_node = NULL; 


for(face = polyhedron->obstacle; 
face != NULL; 
face = face->next_face) { 


node = face->face_nodes; 
if(node->pt.z != node->ccw->pt.z && 
node->pt.z != node->cw->pt.z) continue; 

else[ 
if(NULL -- min vert face) min vert face - face; 
else[( 
if(min vert face-»face nodes-»pt.z « node-»pt.z) break; 
else min vert face - face; 
) 

) 

) 


return(min vert face); 


) 


polygon list *make list(to, from) 
obstacle plane *to, *from; 


[ 
polygon list *polygons; 


NEW POLYGON(polygons); 
NEW POLYGON(polygons-»next polygon); 


polygons->polygon start - from; 
polygons->next_polygon->polygon_ start = to; 
polygons->next_polygon->next polygon = NULL; 


return(polygons); 


] 
int free pseudo polygon list(polygons) 


polygon list *polygons; 


free(polygons-»next polygon); 
free(polygons); 
return(1); 


} 
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int chech tangent list(tangent list, path) 
a path *tangent list; 
paths *path; 


double dir; 


if(path->forward) dir = goal direction; 
else dir - norm(goal direction - HPI); 


if(ABS(norm(dir - 
atan2(tangent list->pt.y - path->last node->pt.y, 
tangent list->pt.x - path->last_node->pt.x))) >-HPI) 
return(0); 


if(abs(norm(dir - 
atan2(tangent_list->next_node->pt.y - path->last_node->pt.y, 
tangent_list->next_node->pt.x - path->last_node->pt.x))) >= HPI) 
return(0); 


return(1); 


) 


a path *free tangent list(list) 
a path *list; 
[ 


a path *previous, *current; 


for(current = list, previous = list; 
current != NULL; 
previous - current, current - current->next node, free(previous)) 


free(previous); 
return(NULL); 
] 


polygon list *free polygon list(list) 
polygon list *list; 
[ 


polygon list *previous poly, *current poly; 
obstacle plane *node; 
int node number, count; 


for(current poly = list, previous poly = list; 
NULL != current poly 


previous poly - current poly, current poly - current poly-»next polygon, 


free(previous poly))[ 


for(node - current poly->polygon start, 
node number - current poly-»polygon start-»number nodes, 
count = 1; 
count <= node number; 
node = node->ccw, node->cw->ccw = NULL, free(node->cw), count++); 
] 
free(previous poly); 
return(NULL); 
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APPENDIX F 


This appendix contains the source code found in path.c 


an lude "3d tan.h" 
include "plot.h" 


define LOOPFOREVER 1 
define AROUND PATH 1 


n—————————MM—————-—-———. ________ 
REMOVE PATHS 

Mi a a SE. I |. 

This function frees from memory the created possible paths so that another 

iteration of path building can commence. It accepts the head of the list 


and returns a NULL pointer to the head of the list.*/ 


paths *remove paths(start, shortest) 
paths ‘staat, ) Shortest; 
{ 
paths Seurremie— SLame—-next path, “previous = start; 
a_path anode noder; 
int i; 
lor revsousS = stare current = starbt-»next path; 
Eurrentzonexb pathoe- NUDBL; 
|Eevsousds-scurrent,Scurrest.z current-»next path) ( 
if (shortest path -- previous) { 


shortest->next path = NULL; 
continue; 


) 


for (node = previous->last_node, nodel = node->next_node; 
nodes t - NULL; 
node = nodel, nodel - nodel-»next node) 


free (node); 


free (node); /* frees last node in the path being deleted */ 
free(previous); 
) 
Ew shortest path !—- current) | 
previous - current; 
for (node - previous-»last node, nodel - node-»next node; 
nodel != NULL; 
node = nodel, nodel = nodel->next_ node) 


free (node) ; 


free (node); /* frees last node in the path being deleted */ 
free (previous); /* frees last path in the path list */ 

} 

Start = shortest; 


return (start); 
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ORDER 

E O O DO O oleo na 
This function uses the area function for a triangle in determining if the 
current node is the common tangent. It uses one of the two adjacent nodes 
and the inputted point to form the triangle. It returns the value found 
(double) in the calculations.*/ 
double order (pl, current, adjacent node) 

Point p 

obstacle plane *current, *adjacent node; 
( 

double Z 

z = (current->pt.x - pl.x) * (adjacentinodes= pi r E 

z= z - (adjacent noder>pt.x - pl.x) = (Currents pty MP EE 


returna) 


This function finds all the valid tangential lines from the start point 

to polygons in the start 2d listland places theming he starii po SEE 
list. It returns this link list to the calling TUCC Cion De puncti 
calls plus and minus tangent functions to find the tangents from the start 
to all the polygons in the start_2d list. As it finds the two tangents 
for a polygon, it calls cross polygon to determine ifi chercemis Te CNS a 
gon between the point and the start point. If not, then the node is added 
to the list as the beginning of a valid path, by calling Create pacho 
It calls assign point values, which places the new point into the DU 
The function also calls the function atan2, which is a library function that 
determines the direction from the start point to the node added to the 

start paths“ St n / 


paths *start finding paths(start paths, Start twod starii cca 
boundry, active node list) 
paths *start_paths; » D 
polygon list “Start LWOd; 
point start pt, goal, 
boundries boundry; 
active nodes *active node list; 
( 
paths *current path - NULL; 


polygon list “current ipo gon; 
obstacle plane *current_node; 


int count, tangent; 

point ptl, pt2; 

plane_number = 1; /* tells what plane the 2d representation is 
ona 


vertical_n perpendicular plane(start point, goal); 
/*start_2d is a global defining the two d polygon list*/ 
start 2d - build two d polygon list(start 2d, perpendicular, FALSE); 


#ifdef DOS 
if (plot2screen) 
datavp Ot (stare. 2d), 
#endif 


for (current_polygon = start 2d; 
current polygon !- NULL; 
current polygon - current polygon-»next polygon) { 


ptl = plus tangent(current polygon-»polygon start, start pt); 
pt2 = minus tangent (current polygon-»polygon start, start pt); 
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if (direction heuristic from start(start pt, ptl) && 
boundry check(beundry, pti)) 1 
UE(Icross polygen Start 2d, start pr, pt!) }( 


start paths - create path n nodes(start paths, current path); 
tangent - PLUS; 
if (current path -- NULL) 
current path - start paths; 
ENSE 


cur Pa current path—>next path; 
current par sipdate Current path (Current path, /*start_paths, */ 
tangent, start pt, ptl, goal, 
current polygon -polygon number); 
cürrent path- add start point ce path(ceurrent path, start pt); 


#ifdef DOS 
if (plot2screen) 
drawaeline segment (Start pe, current path->last node->pt, YELLOW, 
a REGULAR) ; 
tendif 
) 
else( 
ie horizontal) 
start paths — up and over(start paths, current path, start point, 
pebNsturteporsnt, PLUS, 
current polygon-»polyhedron number); 
Pc Urren ipat == NULL current path = start paths; 
else SCUrrEenE path - current path->nexXxt pach; 
current path- ~forward = YES; 
} 
) 


Rr ection heuristic from start(start pt, ptê) && 
bounary check(beundry, pt2) )' 
Di (temess polygentstart 2d, (Start pt, pt2))( 

SH ans ea pan nedes (Start pathisf current path); 

tangent - MINUS; 

if (current path == NULL) 

curPentopaPm -Start paths, 
else 
current path= current patn=>next path; 

current pachi UE current patnl(curMent path, tangent, start pt, 
Pic, goal, 
current polygon->polygon number) ; 

cürrent path = add start Point to path(current_ path, start_pt); 


fifdef DOS 
if (plot2screen) 
draw line segment(start pt, current path->last_ node->pt, YELLOW, 
REGULAR) ; 
tendif 
) 
else( 
Aar(tnorizontal)i 
start paths - up and over(start paths, current path, start point, 
pu Start point, MINUS, 
current polygon->polyhedron number); 
if(current path == NULL) current path = start paths; 
else cufrent path = current path->next_ path; 
cüūrrent patnìn- -forward = YES? 
} 
) 
) 
) 


his function finds the one partial vertical path from the start 
point over all obstacles to goal*/ 
Start paths = Up andafover (start paths, current path, start point, goal, 
start point, VERTICAL); 
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current_path->next_path->forward = YES; /*current _Path- >next is path 

added to the list that is 
up and over all obstacles*/ 

current path - current path-»next path; 

/*this function finds all the partial vertical paths from the goal, thus 
working backwards to find all the paths which go around one or more 
obstacles and over at least one obstacles*/ 

start paths - up and over from goal(start paths, current path, goal, start 2d); 


return (start paths); 


This function adds the start point to the partial list being created by 
the function start findlmc bethe 


paths *add start point to path u'r em: pac i u'r a O | 
paths *cūrrent pa h; 
point start point: 
( 
a path *currentinoder 
for (current node — current path-»last node; 
current node->nexrt nodem = UOCE 
current node >= current nodes -ncxemnoa i, 


NEW NODES (current _ nodez> next node),; 


current node->next node->pt = assign _point values (start point); 
currenti "node-»next node->next node = NULL; 
current node->next _node->next_ after node = current node; 


return (current _path); 


This function is called to create and insert into the start paths link 
list new points which form valid paths from the start.*/ 


paths *create path" modes startWpsths current Pach) 
paths xL paths, *CUPreEnt pathi 

( 
if (start paths == NULL) ( 


NEW PATH(start paths); 
current path — Sar ne 
) 
else ( 
NEW PATH(Current path-c>nex' Spa hM: 
current path = current pacha- nezan hu. 
) 
current path-»polygon4vert PTR = NULL; 
current path-»next path = NULL; 
NEW NODES(current path-»last node); 
current _path->last_ node->next _node = NULL; 
return (start _paths); 


104 


This function determines if a line crosses the polygon. It checks each 
line segment of the polygon and returns 1 if there is an intersecttion. 
The second for structure is used to check if p2 is on a polygon. If so, 
boolean is asserted to true (int value - l). This tells the rest of the 
more loop to go on to the next polygon*/ 


MNAE Gross polygon (Start, pl, pz) 
polygon list eStart; 
point pl, pz; 

{ 
ant count, boolean; 


polygon list *polygon = NULL; 
ee tacle plane *current = NULL; 


point po, pd; 
double WX, Yr Z? 
for (polygon = start; polygon != NULL; 


polygon = polygon->next_polygon) ( 


Bor counto-s current -opolygon-»polygon start, boolean - FALSE; 
count «- polygon-»polygon start-»number nodes; 


zounti, cubprent -current-»5ccw) ( 

Tm 2x —— cuübbeunt*2pt.x 45 pZ.y == current-=>pt.y) { 
boolean = TRUE; 
break; 


) 


if (!boolean) ( 
for (count = 1, eurrent = polygon=>polygon start: 
count <= polygon-»polygon start-»number nodes; 
Count++, current = current=>ccw) { 
Pow) = Ccurrent=>pL.Xx,/ p3.y = Gurrent->pt.y; 
p4.x — current—->ccw—>pt.x, p4.y —- current-»ccw-»pt.y; 


W siga Eunction one(b  XbJ.y, Dl, pê)); 
x = sign(function one(p4.x, p4.y, pl, p2)); 
y — Sign(&anction two(pl.x, pl.y, p3, p4)); 
z = sign(function two(p2.x, p2.y, ps, pad); 
if (w != x && y != z) ( 


return (1); /* line intersects polygon */ 
} 
} 
current o-— current-»2CCW; 
) 
) 
return: (07 


Funcitons one and two are simple line equaitons used to determine if two 
lines intersect. They both return the value of the equation to the 
rang funcion*/ 


double Bunctrongsne(x, y, pl, p2) 
double xX; y: 
point Ply Be? 

( 
HE ZU 


PEN y — p vM P en») - “(py pl) * (x — pl.x)); 
return (zZ); 
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) 


double function two(x, Y, PINES 


double X, Y: 
point p3; p4; 
{ 
int 2 = 0; 
z= ((y - p3.y) * (p4.x = p3.x)) = (PI IX e 


retürn9tz)t 


This function accepts from the calling function a valid path, duplicates 
it and inserts the duplication into the start paths link list. It calls 
duplicate entries to duplicate the header data of each path (ie type of 


tangent, total cost, estimated cost, direc ion ecca) 
paths *dúuplicatelpaths (current) 
paths *current; 
( 
a path *node, *duplicate node; 
paths *dupincate:; 
duplicate = duplicate entries veurrene): 
for (node — current=>lastemede; 
node != NULL; 
node = node->next_node) ( 
if (duplicate->last_node == NULL) ( 


NEW NODES(duplicate->last_node); 
duplicate node - duplicate—>last node; 
duplicate node->next after node - NULL; 
) 
else ( 
NEW NODES(duplicate node->next node); 
duplicate node-»next node-»next after node - duplicate node; 
duplicate node = duplicate node=>next node; 
} 
duplicate node-»next node - NULLI; 
duplicate node-»polygon number - node-»polygon number; 
duplicate node-?»pt - assign point values (node-»pt); 
} 


return (duplicate); 


This function duplicates the major entries of a valid path needed in the 
calculations for finding the best path*/ 


paths *duplicate entries (current) 
paths ‘current; 

{ 
paths “duplicate, 


NEW PATH (duplicate); 

duplicate->last_ node = NULL; 

duplicate->cost - current->cost; 
duplicate->estimated cost - current->estimated cost; 
duplicate->total cost = current->total cost; (NW 
duplicate->ray direction - current->ray direction; 
duplicate->plane_number - current->plane number; 
duplicate->type tangent = current->type tangent; 
duplicate->forward = current->forward; 
duplicate->polygon4vert PTR = NULL; 
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meturn (duplicate); 


This function accepts three parameters (the start path for either possible 
verticle paths of possible perpendicular paths, the start pointer for the 
2d polygon representation, and the goal point. It searches the possible 
path list for the shortest total distance from start to goal. It checks 
the line projection of this path and the goal with the goal point for any 
Eutersection with polygons from the polygon list (start 2d list). If no 
intersections, the goal is appended to this shortest path and the short- 
est path is returned to the the calling funcition. If there is an inter- 
section, the function calls FIND TANGENTS FROM INNER NODES, passing to it 
the shortest path and the start 2d pointer 

a 


paths rez enda anu Sa goa we shortest path, start 2d, boundry, 
active node list) 
Polygon list Astart 2d; 


point start, goal; 
paths *shortestťt path; 
boundries boundry; 
active_nodes *active node list; 
( 
paths *current path = start path; 
a path hnewErecord current; 
if (shortest path->type tangent != NONE) { 


shortest path = find tangents from inner node (shortest path, start 2d, 
boundry, active node list, 
1); 
shortest_path->able2expand = NO; 


} 
else { 
if(shortest path->forward == YES) /*partial path is from start or 
forward*/ 
start 2d -= add on new plane intersection (startwezd, 
hortes path >last node, 
goal); 
else /*partial path is from the goal or backwards*/ 
start 2d = add on new plane intersection(start 2d, 
shortest path->last node, 
Start) ; 


shortest path->plane number = plane number; 
shortest path - up around up(shortest path, PLUS, start point, goal); 


shortest path - continue vertical path(shortest path, start 2d); 
) 
return shortest path); 


This function appends the goal point to the shortest path if the last node 
of the shortest path list and the goal point are visible to each other. 

it then returns the entire list of possible paths back to the calling 
Nunction. */ 


paths o eate goal, shortest, partial path) 
point goal; 
paths *shortest; 
a path *partial path; 
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a path *new record, “Lemp, 
double cost. = 0; 


#ifdef DOS 
if (!plot2screen) 
printf("\nYou have reached the goal\n"); 


telif 
printf ("AnYou have reached the goalin"); 
fendif 
for(temp = partial path; temp->next_node != NULL; 
temp - temp-»next node) 
cost += distance(temp->pt, temp->next_node->pt); 
temp-»next node - shortest->last_node; 


shortest-»last node - partial path; 
cost += distance (temp->pt, temp->next_node->pt); 
shortest->cost += cost; 


NEW NODES(new record); 

new record-»pt = assign _point_values (goal); 

shortest->cost += distance (shortest->last_node->pt, new _record->pt); 
new_record->next_node = shortest->last_node; 

shortest—>last nédé = —gnewr record, 

shortest —>total  cest@=@snecrtest] cost, 

shortest=->estimated costi mu, 

shortest —>type Cangent NONE 


return (shortest); 


f p e—————————————————————— i o. 
FIND TANGENTS FROM INNER NODES 

=-=. 00 On aaa a a a ee eee 

This function finds all valid tangents from an inner node to other poly- 

gons. It is very similiar to thewsteEt findingipaths function Ou er 


it checks the direction of the possible new node of the path with the 
current direction of the path to determine if the node is valid. It also 
calls a variation of cross polygon (cross polygon ncde on) Enat takes 
into consideration that both the" node that form a line lie on a polygon. / 


paths *find tangents from inner node(shortest, start 2d, boundry, 
head node list, recursion number) 
paths *shortest; = = 7 
polygon list *start 2d; 
boundries boundry; 
active nodes ‘head node lise, 
int recursion number; 


polygon_list *polygon, “previous, “pelyconl: 


paths *expanded, *same_polygon = NULL; 
a path “new record; 
#ifdef DOS 
a path *print path: 
#endif 
point ptis pt25 
a path "current = shortest->last node, 


*minus tangent list, 
“plus tangenta iist 


for (polygonl - start 2d;/* finds the polygon the last node of the 
* shortest path lies on */ 
polygonl->polygon number !- shortest->last node->polygon number || 
polygonl-»plane number !- shortest-»plane number; o 
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polygonl - polygonl->next polygon); 


ben (polygon = stare 2d; 
polygon != NULL; 
polygon = polygon->next polygon) { 


on_same polygon = FALSE; 
if(shortest->plane number < polygon->plane number) 
/*planes found after the plane which shortest path last node 


break; 
is are not checked*/ 
yf (shortest->plane number != polygon->plane number || 
shortest-»last node-»polygon number -- polygon->polygon number) 
continue; 


minus tangent Jism — tangent (shortest, polygonl, polygon->polygon start, 
MINUS, start 2d, AROUND PATH); 

Plus tangent ist = Cangent (shortest, polygoni, polygon->polygon_start, 
BBUS, Stase. 2d, AROUND PATH); 


EE directronwmheurrssticdtangent(shortest, plus tangent list) && 
boundryscheckiboundry, plus-tangent list—>pt) && 
boandmscpncckiboundrv phiusetangent list—>next node->pt)) | 

if(NULL !- plus tangent list && 

Mee wactivemmode lust (head active nodes, shortest, 
plus tangent list->pt)) ( 


#ifdef DOS 
if (plot2screen) { 
Pow(Orintepathe—- plus tangent list; print path->next node !- NULL; 
printepathe- printopath-^2nextonode) 
draw line segment(print path-»5pt, print path-»next node-»pt, YELLOW, 
REGULAR); 
Granelinewseqment (print path—=>pL, shortest—>last node—>pt, YELLOW, 
REGULAR) ; 
} 
#endif 


paths foundt+; 

expanded = duplicate paths (shortest) ; 

Snowmen expand paths with values(shortest, expanded, 

plusgEbdncent9elstoshead node list, 
polygon->polygon number, PLUS); 


if (on same polygon) 
same polygon - expanded; 
) 
) 


if (direction heuristic4tangent(shortest, minus tangent list) && 
boundry check(boundry, minus tangent list-»5pt) && 
boundry check(boundry, minus tangent list-»next node-»pt)) ( 
if (NULL != minus tangent list && 
eneckfactaive mode st(head active nedes, shortest, 
minus tangent list-»pt)) ( 


#ifdef DOS 
if (plot2screen) { 
[orp aie “nus tangent list; print path->next node !- NULL; 
print_path = print path->next node) 
draw_line segment (print _path->pt, print path->next_node->pt, YELLOW, 
REGULAR); 
draw line segment(print path-»pt, shortest-»last node->pt, YELLOW, 
REGULAR); 
) 
#endif 


paths founds, 
expanded = duplicate paths (shortest) ; 
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shortest - expand paths with values(shortest, expanded, 
minus tangent list, head node list, 
polygon-»polygon number, MINUS); 
if (on same polygon) 
same polygon = expanded; 
} 
} 
} 


return (shortest); 


fr + a a ee eee 
- CHECK FOR NODE ON POLYGON 
rn MMMM A eS ee 
This function determines if a point is on a polygon. Since all the 
polygons lie on the same plane, depth was not considered in this function 
and the z value of the point was not used. It returns a one if the node 
is on a polygon, else a zero*/ 
int check for node on polygonpt, (current, 
point pt; 
obstacle plane "Cubs 
( 
if (pt.x == current—-pe. 2744 
pt.y == current->pt.y && 
pt.z == current-2pt2z) 


returned s 


lf (pt.x ss ocurrent-»CCW- DES XC 6 
pt.y == current->ccw->pt.y 4£ 
pt.z == current-2CccWzopt o2] 

return (1); 
else 


return (0); 


This function removes, clears from memory the path marked as the shortest. 
In doing so, it reestablishes the link list and returns to the calling 
function the start of the path list.*/ 


paths *removegshortest path (shortest, (Start pac} 
paths *shortest, "spar Ih 

( 
paths "current, “previous, 
a path *current node, “Pprevicucenode, 


for (current = start_path; 


current {= shortest, 

previous = current, current - current->next path); 
if (current == start path) 

Start path = current next. 
else ( 


previous->next_path - current->next path; 


current->next path - NULL; 

previous_node - current->last node; 

current node - previous node; 

free (current); B 

do ( 
current node - current node-»next node; 
free(previous node); d 
previous node - current node; 

} while (previous node != NULL); 
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return (start path); 


/* ee a a a e e e a e e e e X e e e e e e a O A a e e e e e a e a e e e e ID Y ODD a a e 
CROSS POLYGON NODE ON 

== A —————--—--— UU SEEN. | ÎŴ Ŵr 

This function determines if a line crosses the polygon. It checks each 


line segment of the polygon and returns 1 if there is an intersection. 
The first for structure is used to check if p2 is on a polygon. If so, 
boolean is asserted to true (int value - 1). This tells the rest of the 
BSTEIOODp to go on to the next polygon*/ 


int cross polygon nece on(start, pl, p2, plane number) 
polygon list Starts 
point pl, p2; 
Bm plane number; 
( 
int count, boolean = FALSE; 


polygon list *polygon = NULL; 
obstacle plane *current = NULL; 
point p3, på: 

double W X; Yr 2? 


for (polygon = start; polygon != NULL; 
polygon - polygon->next polygon) ( 


for (count = 1, current = polygon->polygon start, boolean = FALSE; 
count <- polygon->polygon start->number nodes; 


count++, current = current->ccw) ( 
if (polygon->plane number != plane number) { 
boolean = TRUE; 
break; 
} 
MI p2?.x —ucurrent-»pt.x && p2.y == current-»pt.y) ( 
boolean - TRUE; 
break; 


) 
) 


+1 (boolean) ( 


lor count — 1, current = polygon=>polygon start; 
count <= polygon->polygon _start->number_nodes; 
count++, current = current->ccw) { 


ps -scurrent-cptex psy -somsrent-»pt.y; 
p4.x current CCW DL. PAY — current >ccw—>pt y? 


YE uen peU. ae pl.y == current—>pt.y) |l (pl.x 
= R Orren Cew= pikes pl.y -- current--ccw-»pt.y)) 
continue; 


“Eco cn mc cu Ee DE pt, currenn—>ccw—>pt, pl)) ( 
continue; /* skips over pl which is on the polygon */ 


) 


wc gn Eun- Mon ene(p3.x, p3.Y, pl, p2)); 
x = Sigm(tumectonmone(p4.x, p4.y, pl, p2)); 
y sIgn EUR On EWO(pl.x, pl.y, p3, p4)); 
z = Sign (funceronstwo(pZ.x, p2Z.y¥, p3;,. p4) ); 
if (w != x && y != 2) 


return side /* line intersects polygon */ 
} 


current = current—>ccw; 


) 


UD 


return (0) 


This function identifies the shortest path and reverses it so it is from 
start to goal vice goal to start order. The function builds a new path 
in doing so returns the path with a pointer labeled reverse path*/ 


paths *reversed list (path _ list) 
paths ‘path: lice: 
{ 
paths *reversed path; 
a path *current path list, *current reversed path, *new record; 


reversed path - duplicate entries(path list); 


NEW NODES (reversed path-»last node); 

current reversed path - reversed path-»last node; 

current reversed path-»pt - assign point values(start point); 
current reversed path-»next node - NULL; 


for (current path list = path lstce>l<S' “med 
current path list !- NULL; 
current path list -~ Cürrent path Psi -next mn le a 


NEW_NODES (new_record); 
new record-»pt - assign point values(current path list-»^pt); 
new "record-»next node = Current reversed | path- »next node; 
current reversed path-»next node - new record; 

) 

return (reverscdWp Gt 


This function determines if the new node to be added to the start paths 
list is valid. A node is valid if the direction from the start to the 
node is within 90 degrees of the start to goal direction. The initial pt 
can either be the start point or the goal point. If it is the goal pt 
then the goal direction is normalized to goal direction minus 180 degrees 


a 


int direction heuristic from start(expand, pt) 
point expand, pt; 

( 
double ray direction, a, goal dir - goal direction; 
if(goal.x -- expand.x && goal.y -- expand.y && goal.z -- expand.z) 


goal dir = norm(goal dirmi BU 


ray direction - atan2((pt.y - expand.y), (pt.x - expand.x)); 
a= "ABS (norm(ray direction = goal dirii: 
if (a <= HPI) 
returna OU 
else 
return (0); 


/* ———— —— —Ó——— ——Ó———Ó—À—— —Ó——— ———À — ——À —— ————À———————————————————————————————————-— 
DIRECTION HEURISTIC 

nnn cu ilz rr "mane ik O k odoO---="+=====" “a O 

This function determines if the new node to be added to a valid path is 

in the correct direction. If the type of tangent to the last node is a 
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plus tangent, the the new direction minus the old direction must be less 
than or equal to zero. Vice-a-versa for a minus tangens*/ 


Int direction heuristic(current, pt) 
paths *current; 
point pt; 
( 
double ray direction, a, goal dir - goal direction; 
a path Egogcccurrent--lastoneder 


if(!current->forward) 
goal dir = norm(goal dir + Pl); 


Hay mdlrection = atan2((pt.y = node=>pL.y), 
ipt x = Node- ex) ); 
a 2oy direction goal dir; 
a norm(a) ; 
if (ABS(a) > HPI) 
return (0); 


(Wm norm(ray direction - current->ray direction); 
if (ABS(a) > PI / 2) 
return (0); 
ig else 
return (1) .*/ 


if (current—>type tangent == PLUS) { 

Tf (nerm(ray direction — current—>ray direction) <= 0) 
return (1); 
else 


return (0); 
) 
Ee 
if (norm(ray direction - current->ray direction) >= 0) 
Eecurn (1); 
SL 
teturn (O); 


AA P.D. Ll. LL. LL LL. O e 
NORM 
MM a SD OU Se SSS SS SSS SS SS SSS SS SS 
This function normalizes an angle between -PI and PI.*/ 
double norm(a) 
double a; 
{ 
while ((a > PI) ||] (a <= -PI)) { 
ii (a > PI) 
a= a = DPI; 
else 


a =a + DPI; 
) 


return (a); 


This function updates the path information associated with the path 
pointed at by the current path pointer. After updating the info, the 
function returns the start path pointer back to the calling function.*/ 


paths *update current path(current path, /*start path,*/ tangent, 
start pt, pt, goal, polygon number) 
paths teurrentcipath/*, *start path*/; 


S 


point 
Inte 


Start pt, 
tangent, 


pt, 


current path-»lastenedec pb 


current path->last _node->polygon_ number 
current _path- -»last node-»next after node 
distance(start pt, 


current _ path- >cost 
current path->estimated cost 
current path > toral cost 
current _path- >type tangente 

current path->ray_ direction 


goal; 


polygon number; 


assign point values(pt); 
polygon number; 
NULL; 

pt); 
distance(goal, pt); 


TOTAL COST; 


tangent; 
atan2í((pt.y — startEDE NP 


(pt.x = start pt xiii 


current_path->plane_ number 
current_path- >able2expand 
current path->forward YES; 
head active nodes 


— 


curren 


insert_ nodes into active node list(head+active nodes, 


plane number; 


TRUE; 


pt, 
LBath ces: 


current path->when path found 
return (currenti paroii 


paths_found++; 


— e e cee ce ee a a ca E  — — cr cr cr cr cr ce ee Y — cr e cr cc e cr — — S SS y a we — —— Sd — — — 


EXPAND PATHS WITH VALUES 
This function recalculates the header data of the duplicate record of the 
shortest path to include the new point to be added (hence expanded as the 
variable name). It also inserts this expanded path into the link list of 
valid paths after the shortest path and returns the shortest path to the 


calling fonce lone 


paths *expand paths with values(shortest, expanded, tangent list, 
head active, polygon number, mode) 

paths *shortest, *expanded; B 

a path *tangent list; 

active_nodes *head active; 

Ine polygon number, mode; 
( 

a path *current = shortest->last_node, *to polygon qe 
tangentelrst-»nextonode; 

UTE last tangent type = expanñasd=>type cangen 

if (shortest->last node->pt.x to polygon—>pt.x && 


shortest->last node->pt.y 
shortest->last _node->pt. Zz 


expanded->cost += distance 
free(to polygon); 
tangent_list->next node 
expanded->last node->next 
expanded->last_node tang 


) 
else ( 
expanded 
last tangent type); 
) 


EX 


expanded->cost += 


connect links of path on polygon (expanded, 


to polygon->pt.y && 
to polygon pee) ot 
(shortest->last node-—>pt, tangent IS: PO 
expanded->last node; 


after mede tangent list; 
ent lus dee 


tangent list; 


distancle(shortest->last (node => ype 


shortest-»last node-»next node-»pt); 


* 
/ 
if (expanded->forward) 
expanded->estimated cost 
else 
expanded->estimated cost 
expanded->total cost 
expanded->ray direction 


— 
— 


— 


distance(start point, 
expanded->cost + expanded->estimated cost; 
atan2 ((expanded-=>last nodes>pt.y > 

expanded- 


distance(goal,expanded-»last node-»pt); 


expanded->last node->pt); 


>ïast node >nexw'"'Hode- py 
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texpanded-»last node—-»pb.x — 
expanded- -»last node=>next node->pt.X)); 
expanded->able2expand = TRUE; 
expanded->type tangent = mode; 
Mead active = insert des into active node list(head active, 
expanded->last_node- >pt, expanded->cost); 


/* 
expanded = update (expanded, mode); 
E 
expanded->when path found - paths foundt*t; 


ESpanded-»next path --shortest-»next path; 
expanded-»last node-»polygon number - polygon number; 
shortest-»next path - expanded; 


return (shortest); 


This function finds the vertical and perpendicular planes at the point 
passed in. The point is the vertical point to be expanded from. Once 
nas these planes, the function calls the build two d polygon list, 
which determines the polygons formed by the intersection of the new 
perpendicular plane with each polyhedron in the search space.*/ 


polygon list fadduenmncwnplane intersection (start 2d, 
E vertical pt list, goal) 
polygon list *start 2d; 
a path © *vertical_pt list; 
Point goal; 


polygon list *last polygon; 
a path Secusrcente- vertical pt ist; 


vertical n perpendicular plane(vertical pt list->pt, goal); 
start par build ewo d polygon List (start | 2d, perpendicular, TRUE); 


Bccurn o (start 2d); 


/* —————— S a a m SEE. UN e 

POINT ON POLYGON 
--—-—-----------------------2------------ O00------------------------------- 
This function determines if a point lies on a line. The function is used 
to skip the polyhedron edge which the vertical point expanded from lies 
on. It is also is used as a heuristic to determine if a new vertical 


point lies between the goal point and the current point being expanded 
an- */ 


int Point son line (pt, pt2, pt3) 
point PL DD pr. ptl 2nd pt2 are end points of 
* line, pt3 is point being checked */ 


double LYW ES: 


if (pt2.x == ptl.x) { 

if “pes .x t= set x) 

return (O) 

else ( 

Ef ((pELOVo ps2 v) && (ptl.z !9 pt2.2))- ( 
t2 c (proLEDLISy) / (pt2.vy — ptl.y):; 
t3 Oz) (pt2,2.— ptl.z); 
1£ (12S=5t3) 

return (1) 
else 


o 


return (0); 
) 
else ( 
if (pty ~=" PE. y 
if (pt3 SS DEL.) 
return (ly; 
else { 
t3 5 (pt3.z = ptl 2) / (pPE2. 2 eee 
if (0 <= C) 50 tc) 
return (1); 
else 
return (0); 
) 
) 
else { 
TEM DEL. 2 e=— 3 eZ 2) 
LE dpel.z '— Po) 
return (0); 
else ( 
t2 = (pt3.y = ptl.y) 74 0pB2 V upto 
if (0 <= t2 && t2 <= 1) 
return e 
else 
return (O) 


) 
else ( 
if (ptz.y == PEL 
if (pt3.v!— Et 
returm GO 
else { 
if (ptl:z lS pe ee 
tl = (PESTE ES 
t3 = (pt3.2 = seule 2) “r.n 


if (tl == t3} 
return (1}; 
else 


return (0); 
) 
else { 
if Mptl SZ, == oes a2 NI 
1f (ptamz == ptiz) 
tl = (pt3.x = pil. x), Es ENDS 
if (0 <> tt] se t|lE<— |) 
return (ll); 
else 
return (0); 
} 


} 
} 
else { 
LE (pede z ==" pt2. 2) a 
Depts ec l= pu |! z) 
return (OD); 
) 
else ( 
tl = (pt3.x - ptl.x) / (pt2:x =S. PEE 
t2 = (pt3.y —- ptl.y) / (pt2.V — pu OE 
abe ic I 0:2) 
return (1); 
else { 
= (pt3.x - ptl.x) 7 (pt2 XE RE ERE 


eal 
t2 (pt3.y — ptl.y) 7 (pt2 ^v epee 
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Co ae pee ceeemerl.2z) / (pt2.z — pel. z); 


AE AS) 
return (1): 
else 


return (0); 


) 
) 
) 


return (0); 


This function connects the links of a path on the polygon. The tangent 
function finds the either a plus or minus tangent between polygons. This 
tangent connects the path between the last node of the expanded path and 
the tangent node which lies on the same polygon as that last node.*/ 


paths *connect links _ of path_on polygentexpanded, tangent_list, mode) 
paths *expanded; 
a path heangent list; 
int mode; 
{ 
a_path *last node in tangent - tangent list-»next node; 


polygon list *polygen; 
aestacle plane *node; 
point extend; 


extend = expanded->last_node->pt; 


last_node_in_tangent->polygon_number = expanded->last_node->polygon_number; 


Peecinds correct polygon points lie on */ 

for (polygon = start 2d; 
polygon->polygon number t= expanded->last node->polygon number; 
polygon - polygon->next polygon); o E 


/* 


* finds the correct node in polygon which is equal to last node in 
* tangent list 


E 
for (node - polygon->polygon_ start; 
mode >ptux Je Mast node in tangent-»pt.x | | 
node-»pt.y !- last node in tangent-»pt.y || 
node-»pt.2z !- last node in tangent-»pt.z; 
node - node-»ccw); n 


while (LOOPFOREVER) ( 
if (mode -- PLUS) ( 
if (extend.x == node->cw->pt.x ££ extend.y == node->cw->pt.y && extend.z == 
node->cw->pt.z) 
break; 


cues 
NEW NODES(last node in tangent-»next node); 
last node in tangent-»next node-»next after node - last node in tangent; 
last node in tangent - last node in tangent-»next node; 
last node in tangent-»pt -» assign point values (node-»ccw-»pt); 


) 


node - node-»ccw; 
) 
else ( 
if (extend.x == node->ccw->pt.x && extend.y == node->ccw->pt.y && extend.z 
== node->ccw->pt.z) 
break; 


A 


else ( 
NEW NODES(last_node_ in tangent -nei _node); 
last node in _tangent- »next node-»next after node - last node in tangent; 


last node in tangent - last node in _tangent- 2next node; 

last node in tangent->pt - assign | point | Values (node- >cw—>pt); 
) 
node = node->cw; 


} 


} 
expanded-»cost - calculate cost(tangent list, expanded-»last node, 


expanded->cost, last node in tangent); 
last node in tangent-»next node - expanded->last " node; 
expanded-»last node-»next after node - last node in tangent; 
expanded-»last node - tangent list; 
return (expanded); 


) 


double calculategcost(list, last node in path, Costi lastitangend 
a_path ist last _node_in _path, *last tangent; 
double cost 

{ 
a path *tengent — last tangent, 


cost += distance (last_node _in_path- >pt, last _ tangents REN, 
heag active nodes insert nodes into active node list(head active nodes, 
" last _tangent- >pt, cost); 


for (tangent = last tangents-nextnatcenmnoac, 
tangent != NULL; 
tangent = tangents>next after mnode wi 


cost *- distance(tangent-»pt, tangent-»next node-»pt); 
head active nodes - insert nodes into active node list(head active nodes, 
tangent->pt, cost); 


) 


return (cost); 


MARK SHORTEST LAST NODE 
This function marks the last node's polygon number with the polygon's 
number in which the node was found. 
T 
paths *mark shortest path last node(shortest, aru) 
paths *shortest; 
polygon list *start2; 


polygon list *polygon; 
obstacle plane *node; 
int9WeounDt; 


for (polygon = start2; 
polygon != NULL; 


polygon polygon->next_polygon) { 
if (shortest->plane number != polygon->plane number) 
continue; E 


for (count = 1, node = polygon->polygon_start; 
count <= polygon->polygon _start->number_nodes; 
node = node->ccw, count+t) { 


if (node->pt.x != shortest->last_node->pt.x || 
node->pt.y != shortest->last_node->pt.y || 
node->pt.z != shortest->last node->pt.z) 

continue; 

elsel 


irs 


shortest->last node->polygon number = polygon->polygon number; 
break; 

) 
) 


) 
return(shortest); 


This function accepts a path which has been expanded on and updates 
the header information accordingly, passing back the path and updated 
header info 

E 


paths *update(the path, tan) 
paths *the path; 
Ext tan; 


the path->cost += distance(the path->last_node->pt, 

pn MpPObbe»lasrsnode--next  node-»pt); 
the path->estimated cost = distance(the path->last node->pt, goal); 
the path->total cost - the path->cost + the path->estimated cost; 


Se path >ray direction -— atan2((the path->last_ node->pt.y - 
the path-»last node-»next node-»pt.y), 
(the path- ->last _node->pt. D 
the path->last | node- zunexuEnpde-»pt-x)); 
the path->able2expand = TRUE; 
the_path->type_tangent = tan; 
head active nodes - insert nodes into active node list(head active nodes, 
the path->last node->pt, the path->cost); 


the path->when path found = paths found++; 
the path-»next path - the path-»next path; 
Return (the path); 


) 


int direction_heuristic4tangent (path, list) 
paths *path; 
path *list; 


debble direction, goal dir = goal direction, a; 
hw node = path-»last node; 


if(!path->forward) 
qedar = normigoal dir + PI); 


GE On fatanz((MESt=>pt y" list=>next node->pt.y), 
(list->pt.x - list->next _node- PLA 1) 


a direction — goal dir; 
a norm(a); o 
if (ABS(a) > HPI) 

return (0); 


if (node->pt.x == list->pt.X && 
node->pt.y == list->pt.y && 
mede—>pt.z — liste>pt.z)í 


MO Apart i FeCE iOn ~ direction), 


if(ABS(a) > HPI) 
return (0); 


W9 


return 0i 
) 





APPENDIX G 


This appendix contains the source code to make the simple plots of the world. This 
source code is DOS dependent and is not included if the defined variable DOS is 
equal to 1 during compilation. 


#include "3d_tan.h" 
#include "plot.h" 


#ifdef DOS 

#define ESC Oxlb/* Define the escape key*/ 

#define SCALEDX x / 48 

define SCALEDY y / 34 

#define DOT i| /*used to make a dot with radius l*/ 


void data_plot(start_2d) 
polygon list *start 2d; 
[ 


rnt X, Y, Z, W; 


Initialize(); 
MainWindow( "PLOT OF THE TWO D ENVIRONMENT" ); 


StatusLine( "PRESS ANY KEY TO END" ); 


x  getmaxx(); 
y = getmaxy(); 
ELEStart porptox * SCALEDX; w — start point.y * SCALEDY; 


outtextxy( z, w, "X" y; 

outtextxy(2*20, w*15, "START"); 

Z = goal.x * SCALEDX; w = goal.y * SCALEDY; 
outtextxy( z, w, "X" ); 

outtextxy(z + 20, w*15, "GOAL"); 

draw boundry(); 

draw polygons(start 2d); 


void Initialize(void) 


[ 


int xasp, yasp; /* Used to read the aspect ratio*/ 


if(registerbgidriver(EGAVGA driver) « 0) /*checks for correct driver*/ 


exit(1); 

if(registerbgifont(triplex font) < 0) /*checks for correct font*/ 
exit(1); 

GraphDriver - DETECT; /* Request auto-detection*/ 


initgraph( &GraphDriver, &GraphMode, ""); 
ErrorCode - graphresult();/* Read result of initialization*/ 


if( ErrorCode !- grOk )[/* Error occured during init*/ 
printf(" Graphics System Error: %s\n", grapherrormsg( ErrorCode ) ); 
exit( 1 ); 


) 


getpalette( &palette ); /* Read the palette from board*/ 
MaxColors = getmaxcolor() + 1;/* Read maximum number of colors*/ 


Maxx 
MaxY 


getmaxx(); 
getmaxy(); /* Read size of screen*/ 
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getaspectratio( &xasp, &yasp );/* read the hardware aspect*/ 
AspectRatio - (double)xasp / (double)yasp; /* Get correction factor*/ 


/* */ 
/* PAUSE: Pause until the user enters a keystroke. If the*/ 
/* key is an ESC, then exit program, else simply return.*/ 
443 A 
void Pause(void) 

[ 

static char msg[] = "Pausing. Esc aborts or press a key..."; 
Db oce 
StatusLine( msg ); /* Put msg at bottom of screen*/ 
c = getch(); /* Read a Character from kbd*/ 
if( ESC == Cii /* Does user wish to leave?*/ 
closegraph(); /* Change to text mode*/ 
exit( 1 ); /* Return to OS */ 
] 
if( 0 == c j{ /* Did use hit a non-ASCII key? */ 
c = getch(); /* Read scan code for keyboard*/ 


] 


StatusLine("Working on next expansion"); 


/*cleardevice();*/ /* Clear the screen*/ 


void MainWindow( char *header ) 


[ 


int height; 
cleardevice(); /* Clear graphics screen*/ 
setcolor( MaxColors - 1 );/* Set current color to white*/ 


setviewport( 0, 0, MaxX, MaxY, 1 );/* Open port to full screen*/ 
height = textheight( "H" ); /* Get basic text height */ 


changetextstyle( DEFAULT FONT, HORIZ DIR, 1l ); 
settextjustify( CENTER TEXT, TOP TEXT ); 

outtextxy( MaxX/2, 2, header ); 

setviewport( 0, height+4, MaxX, MaxY-(height+4), 1 ); 
DrawBorder(); 

setviewport( 1, height+5, MaxX-l, MaxY-(height+5), 1 ); 


] 
eu Ay 
/* DRAWBORDER: Draw a solid single line around the current */ 
/* viewport. */ 
e a7 
void DrawBorder(void) 
( 
struct viewporttype vp; 
setcolor( MaxColors - 1l );/* Set current color to white*/ 


setlinestyle( SOLID LINE, 0, NORM WIDTH ); 


getviewsettings( &vp ); 
rectangle( 0, 0, vp.right-vp.left, vp.bottom-vp.top ); 
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] 

7" */ 

/* STATUSLINE: Display a status line at the bottom of the screen.*/ 
ye a 


void StatusLine( char *msg ) 


[ 
int height; 


setviewport( 0, 0, MaxX, MaxY, 1 );/* Open port to full screen*/ 
setcolor( MaxColors - 1l );/* Set current color to white*/ 


changetextstyle( DEFAULT FONT, HORIZ DIR, 1 ); 
settextjustify( CENTER TEXT, TOP TEXT ); 
setlinestyle( SOLID LINE, 0, NORM WIDTH ); 
setfillstyle( EMPTY FILL, O ); 


height = textheight( "H" ); /* Detemine current height */ 
bar( 0, MaxY-(height+4), MaxX, MaxY ); 

rectangle( 0, MaxY-(height+4), MaxX, MaxY ); 

outtextxy( MaxX/2, MaxY-(height+2), msg ); 

setviewport( 1, height+5, MaxX-1, MaxY-(height+5), 1 ); 


) 


void changetextstyle(int font, int direction, int charsize) 


[ 


int ErrorCode; 


graphresult(); /* clear error code*/ 

settextstyle(font, direction, charsize); 

ErrorCode - graphresult();/* check result */ 

if( ErrorCode !- grOk )[/* if error occured*/ 
closegraph(); 
printf(" Graphics System Error: $sAn", grapherrormsg( ErrorCode ) ); 
exit( 1 ); 

] 

} 


void draw boundry() 
int x, y; 


x = getmaxx(); 

y = getmaxy(); 

setlinestyle(SOLID_LINE, OF THICK WIDTH) ; 

setcolor(4); 

line(boundry.x1 * 

boundry.y2 * SCALEDY); 

line(boundry.xl * SCALEDX, boundry.yl * SCALEDY, boundry.x2 * SCALEDX, 

boundry.yl * SCALEDY); 

* 
* 
* 
* 


SCALEDX, boundry.yl * SCALEDY, boundry.xl * SCALEDX, 


line(boundry.x2 SCALEDX, boundry.yl * SCALEDY, boundry.x2 * SCALEDX, 
boundry.y2 * SCALEDY); 

line(boundry.xl SCALEDX, boundry.y2 * SCALEDY, boundry.x2 * SCALEDX, 
boundry.y2 * SCALEDY); 


void draw_polygons(start 2d) 
polygon list *start 2d; 

[ 
polygon list *poly; 
obstacle plane *node; 
int count = 1l, xX, Y; 


x 
Y 


getmaxx(); 
getmaxy(); 


mor 


setlinestyle(SOLID LINE, 0, NORM WIDTH); 
for (poly - start 2d; poly != NULL; poly = poly->next_polygon) { 
for (node = poly->polygon_start, count = 1; 
count «- poly-»polygon start-»number nodes; 
node - node->ccw, counttt)[ 


line(node->pt.x * SCALEDX, node->pt.y * SCALEDY, 
node->ccw->pt.x * SCALEDX, node->ccw->pt.y * SCALEDY); 


void draw shortest path(shortest) 


[ 


paths *shortest; 


paths *path; 

a path *point; 

int x, y, w; Z; SCY te; 
x getmaxx(); 

y getmaxy(); 

style - THICK WIDTH; 

setcolor(14); 

setlinestyle(SOLID LINE, 0, THICK WIDTH); 


for(point - shortest-»1ast node; point !- NULL; 
point - point-»next node)[ 
if(point-»next node !- NULL) 


draw line segment(point-»pt, point-»next node-»pt, YELLOW, style); 
/*(point->pt.x * SCALEDX, point->pt.y * SCALEDY, 
point->next_node->pt.x * SCALEDX, point->next_node->pt.y * SCALEDY);*/ 
circle(point->pt.x * SCALEDX, point->pt.y * SCALEDY, DOT); 
] 
Pause( ); 
cleardevice(); 


void draw line segment(ptl, pt2, color, style) 


} 


point ptl, pt2; 
int color, style; 


int X, Y, W, Z; 
x getmaxx(); 


y - getmaxy(); 
setcolor(color); 


if(color == RED || style) 
setlinestyle(SOLID LINE, 0, THICK WIDTH); 
else 


setlinestyle(DOTTED LINE, 0, NORM WIDTH); 
line(pt2.x * SCALEDX, pt2.y * SCALEDY, ptl.x * SCALEDX, ptl.y * SCALEDY); 
Z = ptl.x * SCALEDX; w - ptl.y * SCALEDY; 
outtextxy( Z, W, "X" ); 
Z = pt2.x * SCALEDX; w = pt2.y * SCALEDY; 
outtextxy( Zz, w, "X" 55 


void id shortest path for expansion(shortest) 


[ 


paths *shortest; 


int x, y "color, i; 
point pl" p2; 
a path *node; 


x 
Y 


getmaxx(); 
getmaxy(); 
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setcolor(BLACK); 
outtextxy(Expansion lable x * SCALEDX, 

Expansion lable y * SCALEDY, "X" ); 
outtextxy(Expansion lable x * SCALEDX + 20, 

Expansion lable y * SCALEDY * 15, "EXPAND"); 


for(i = 1, node = shortest->last_node; 
node-»next node !* NULL; 
node = node->next_node, itt) [ 
node->pt; 
node-»next node-»pt; 


pl 
p2 


draw line segment(pl, p2, RED, REGULAR); 


if(i == 1)( 
Expansion_lable_x paro 
Expansion lable y - pl.y; 
cHEEGxXxExSD (pl x'* SCALEDX, pliy * SCALEDY, “X" J; 
outtextxy(pl.x * SCALEDX + 20, pl.y * SCALEDY + 15, "EXPAND"); 


) 
] 


Pause(); 


) 
#endif 
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APPENDIX H 


This appendix contains the source code in print.c and has only those functions 
associated with printing to either a file or the screen. It also contains some 
print utilities I used in debugging other portions? ot myseoce 


#inelude 23d tan hi 


void print polyhedron(list) 


( 


obstacle list lists 


obstacle list  *current obstacle - polyhedron list; 
polyhedron obstacle aScurren Mace, 

polyhedron obstacle planes currence, 

int 7 counti, count? counts, 


for (countl = 1, current sobs Gace les ice, 
countl <= number polygons; 
Soume lt 


current obstacle = current obstacle->next polygon) { 
printfi("\nObstacle d\n, Count DS 
for (current facet— current obstacle->obstacle, ceuns o — |: 
current. face {= NUL, 
current face = current face nexta cee Ounte 
printf ("Anplane face $din", count2); 
for (current = current face>=- tacemodes, seoune ouster. 
count3 <= current face->face nodes->number nodes; 
count3++, current = current->ccw) { 


printf("Ynx - *.3lf, y = 2.31 i 
current->pt.x, current—>pt.y, cHUELEDEG peas 


void print results(polyhedron list, number polygons, 


start point, goal, plane eguation) 
obstacleslist “pelyhedron ls 


eguation plane equation; 
point start point, goal, 
ine countl, count2, "coulis 


obstacle list ‘*current obstacle — Ppollyhedrene lise, 

polyhedron obstacle *current face; 

polyhedron obstacle plane *current; 

polyhedron _cbstacle plane *ptl = NULL, *pt2 = NULL, “pt3 — NULL; 


for (countl - 1, current obstacle - polyhedron list; 
countl «- number polygons; 
count lc 
current obstacle - current obstacle-»next polygon) { 


printf("VXnObstacle *$d An" count 


for (current_face = current obstacle >c Ea UE, count2 = 1; 
current face != NULL; 
current_face = current_face->next_face, count2++) | 


printf ("Iinplane face $dAn", count2); 
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for (cure cu. ene tace=>face modes, count3 = 1; 
count3 <= current face->face nodes -number nodes; 
count3++, current = current->ccw) { 


Pelnt fee y - 3. aln mz — $.3l1f", 
cun sen EL —currentr pry, current->pt.Zz); 


) 


check */ 

aan tE ("instart point is *.31f, %.31f, %.3lfin", 
Start POI X Start Pont y, start polnt.z); 

nL E("'ngoal 159 31]1f, 3.315, *.3TTWn", 
adal.x, goal.V, gosi7)7 


/* check plane equation for first face */ 

current face = polyhedron list->obstacle; 

for (count2 = 1, current obstacle = polyhedron_ list; 
eurrent obstacle != NULL; 
current obstacle = current_obstacle~->next_polygon, 
current face = polyhedron _list->obstacle} { 


lu cou: -ac-urrentwetacesx current obstacle->obstacle; 
eur ent face- NUEL, 
current lace Current Face->next face, countl++) { 


eint race d\n countl}:; 


ptl = current face->face nodes; 
mee = ptl=>cew; 
PES = ptZ—-ccw; 


PENA ISLE Y ALE) 2 = 108,311", 
EI DD ptl=>pE y, Ptl->pt iz}? 

PINO -— Ol, S lt, 2 = $.31f', 
PE >pDtL.X, pu s>pt "EO --DE.z):; 

EEDD nlr, v'— 6.slf, z= *.31f", 
Desa Dee Pk oso V "pt3-»pt.z); 


plane equation = find plane equation(ptl->pt, pt2->pt, 
pt3-»pt); 
pointi Plane equatilon = <} 3i, 3.3f, *,3f, *.3f>AÔXn", 
plane eguation.x, plane eguation.y, 
plane eguation.z, plane eguation.d); 


— ES Y e YN Y wm I du  —  — YS ee ee Sw - Y dh ee ee ee —— SS ON A e ec ce ce ce ——— Y YI Sd ee ee a ee eee ee 


void print 2d path(start two d) 


polygon list *start two d; 


polygon list Becunrenbpepolvgon s Start Ewo d; 
esstacle plane “current; 
At countlscouhnt2. 


for (countl - 1; eurrent polygon !- NULL; 
current polygon = current polygon->next polygon, countl++) { 
if (current polygon->polygon start == NULL) 
continue; a 


pr'nt Un CEG“ “'nprint ccwAMn", counui); 


bom (CONTA craen — currenu” PS gou--polygon start; 
count2 <= current polygon->polygon_start->number_nodes; 
count2++, current = current->ccw) { 


T 


VO 


printf ("x coord = $8.31f, y coord = Aia coord — $9 NN 
current->pt.X, cCurrent—>pt.Y, CUüUrTEnt a EU 
) 
printf ("print yew ue 
for (count2 = 1, current = current polygons Polygon Stare, 
count2 <- current polygon->polygon start->number nodes; 
count2++, current = current->cw) { 
printf ("x coord = %6-31f, y coord =- Fom E e OE 
current—pt.X current >pt.y ¡Current pz 


id print vertapespsplaneu 
/* all variables are globals */ 

printf("\nThe line intersects the polygon at 0s EL 

intersection.x, intersection.y, intersection.z); 
printf("\nThe vertical plane through the start and goal is\n"); 
printf("*.3fI 4 *.35088 s EN 

vertical.x, vertical.y, vertical.z, vertical.d); 
printf("AnThe plane through the start and goal perpendicular to the"); 
printf(" vertical plane isin A A E 

perpendicular.x, perpendicular.y, 

perpendicular.z, perpendicular.d); 


void print each path(start paths, iteration count) 


paths *start paths; 


paths *Current path, 
int Counc: 


if (!plot2screen || print2file) 4 
if (print2file) 
1f(1 == iterationfcoune) 
fprintf(fpt2, "Wn INITIAL PARTIAL PATHS\n"); 

else 

fprintf (fpt2, "\n\nPATH FINDING ITERATION COUNT = $d", iteration countt-t); 
for (current_path - start paths court - aik 

current_path != NULL; 

count++, current path ="current paris. nextipaco ot 


if (!plot2screen) 
printf("XnNnPATH £dAn” count 
if (print2file) 
Eprinti(fpt2, "nM SPARH Sann", COUDES 


print path(current path); 


void print intersection(plane equation, intersection, ptl, pt2) 


equation plane equation; 
point ptl, pt2, intersection, 


printf("AnThe intersection of the line,"); 
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Eu mI III SS E) Lo 35,2. r5 6.3) inand the plane", 


Et! lA Pt2.x, ptzZ.y, Pez. Z); 


SS a SIR + $.3f) is (8.3, 3.3f, $4.31)", 


plane equation.x, plane equation.y, 
plane eguation: z plane equat ronkad, 
intersection.x, intersection.y, intersection.z); 


A a A A 
PRINT PATH 
Sanna aaa aaa a a SS DD. LL LEE 
void print path(current path) 
paths *current path; 
( 
a path *node; 


if (!plot2screen) { 
printf (“PATH NUMBER $d ", current path->when path found); 
ME current path >able2expand) 
printf ("Path able to extendin"); 
else 
printf("Path cannot be extendedin"); 


if (current path->type tangent == PLUS) 
Lance HPlgsEPSDcent. direction 2 34,3 fin", 
current path->ray direction); 
else B B 
printf("\nMinus Tangent, direction = %4.31lf\n", 
current path -ray direction); 
for (node = current path->last node; 
node != NULL; |= 7 
Hodel nNode=-next node) | 


Tf (node-2next node !- NULL) { 
Prime 3. TD, Roe. 21f) tot; 
node-opt.x,onode-»pt-.y, node—>pt. Z 
printf("\ncost = $4.3lf, est.cost = %4,31f, total - $4 
purrent pathessost, current pathe>estimated cost, 
current path=>total Costi; 


) 
else ( 
prumpr'03 21rT. 959972]£., $3.21f)'' 
node->E “Xx mode->pt.yv, node->pt.z); 
printf("\ncost = $4.3lf, est.cost - $4.3lf, total - %4 
current path-»cost, current path-»estimated cost, 
current path=>total cost; 
) 
) 
) 
ms (printzille) 
print path to file(current path); 


) 


void Print path to filLe(currcent path) 
paths Reurrentiparch; 

( 
a_path *node; 


"uou 


LT; 


fprintf(fpt2, "PATH NUMBER $d ", current path->when path found); 


if(current path->able2expand) 


fprint (Ep E2, “Path able to extend\nw); 
else 


fprintf(fpt2, “Path cannot be extended\n"); 


AD 


fprintf(fpt2, “cost = $4:/31£, feist.cost P MW total = R 
current path-»cost, current paths -ceeinated (cose, 
current paths^total cost); 


if (current path->type tangent == PEGs, 
fprintf(fpt2, "\nPlus Tangent direcion 
current path->ray direction); 
else 
fprintf(fpt2, "\nMinus Tangent, direction = %4.31f\n", 
current pathb=>ray dircectionia 


for (node = current path-»last node; 
node != NULL; 
node = node->next_node) { 
if (node->next node != NULL) { 


fprintf(fpt2, "(23.21f, ê3.211, $3.20) ero wee 
node->pt.x, node->pL.y, nmode--=pe. 4), 
) 


else ( 
fprintf(fpt2, “(S| see eee ee oe 
node->pt.x, node->pt.y, node->pt.z); 


BW TN 7 cc 0090 ===========-- mmc LL 
void print path shervescr(chertest, 

paths *shortest; 

áa path “current, 


if (!plot2screen) 
printf("\nSHORTEST PATH") ; 
if (print2file) 
fprintf(fpt2, “SHORTEST SPAT cs 


for (current = shortest- >last node, 
current != NULL; 
current = current nex Enone E 


if ('plot2sereen) “| 
printf("Nn(s3592]T 0*3 2T] fe EMI 
current-»pt.x, current--pbp y wGcHEBenmse- DUE 
if (current-»next node !- NULL) 
printi rton: 


) 
lf (prunt2frle Wwe 
fprintf(fpt2, "Xn(*$3.2TF, 39 A 
current->pt.x, Current—>pt.v eurr rent: PERA 
if (current->next_node != NULL) 
fpr inti pt? i tol): 
) 


if (!plot2screen) 
printf("\nThe total length of the shortest path is %4.3lfin", 
shorzd5st->totalWcect 
if (print2file) 
fprintf(fpt2, “\nThe total length or the shores rir EEA ie 
shortest=- totalicostik 
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APPENDIX I 


This function contains the source code found in tangent.c and contains only those 
functions which are associated with tangents. 


#include "3d tan.h" 


E.T - QUOI CI eae eg 
TANGENT 

O ÓN A A A 
This function finds the tangent if one exist between two polygons. If any 


obstacles lie between the two polygons, then the function returns NULL. If a 
tangent line is found then the points are placed in a list in euclidean order and 
returned. 


a 
a_path *tangent(shortest, polygon, second polygon start, mode, 
start 2d, type path, mode2) 
paths *shortest; 


polygon list  *polygon; 
obstacle plane *second polygon start; 


int mode; 
polygon list *start 2d; 
int type_path; 
int mode2; 
[ 
a path *tangent path list - NULL; 
point to polygon pt, from polygon pt; 
int i, points found = 0, tan; 


if(type path) 
tan = shortest->type tangent; 
else 
tan = shortest->polygon4vert_PTR->type_tangent; 


if(type path) mode2 = shortest->type tangent; 


if (mode2 /*shortest->type tangent*/ == MINUS) { 
if (mode == MINUS) [ 
for(i = 1; 


i <= second polygon _start->number nodes && 
i <= polygon->polygon_start->number nodes; 
ICT 


to polygon pt - minus tangent(second polygon start, 
shortest->last_node->pt); 
from polygon pt - plus tangent(polygon-»polygon start, to polygon pt); 
if (!cross polygon node on(start 2d, to polygon pt, from polygon pt, 
polygon-»plane number ))[ 
points found = TRUE; 


break; 
] 
] 
) 
else { 
for(i = 1; 
i «- second polygon start-»number nodes && 
i <= polygon-»polygon start-»number nodes; 
i++) ( 
to polygon pt - plus tangent(second polygon start, 
shortest-»last node-»pt); 


from polygon pt - plus tangent(polygon-»polygon start, to polygon pt); 
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if (!tcross polygon node on(start_2d, to polygon pt, 
polygon->plane number))[ 
points found = TRUE; 


from polygon pt, 


break; 
] 
] 
] 
] 
else [ 
if (mode == MINUS) ( 
for(i = 1; 


i «- second polygon start-»number nodes && 

i «- polygon-»polygon start-»number nodes; 

AFON 
to polygon pt - minus tangent(second polygon start, 

shortest->last node->pt); 
from polygon pt - minus tangent(polygon-»polygon start, to polygon pt); 
if (f!cross polygon node on(start 2d, to polygon pt, from polygon pt, 
polygon->plane_number)) { 
points_found = TRUE; 


break; 
] 
] 


else fÍ 
for(i = 1; 
i <= second_polygon_start->number_nodes && 
i <= polygon->polygon_start->number_nodes; 
il 
to_polygon_pt = plus_tangent(second_polygon_start, 
shortest->last_node->pt); 
from polygon pt - minus tangent(polygon-»polygon start, to polygon pt); 
if (!cross polygon node on(start 2d, to polygon pt, from polygon pt, 


polygon->plane_number) ) { 
points found = TRUE; 


break; 
] 
] 
] 
] 
if(points found) 
tangent path list - make tangent list(to polygon pt, 


from polygon pt); 
return (tangent path list); 


a path *make tangent list(to point, from point) 
point to point, from point; 

Í 
a_path list; 


NEW NODES(list); 

NEW NODES(list-»next node); 

list-»next node-»next node - NULL; 
list-»next node-»next after node = list; 
list-»next after node - NULL; 

list-»pt - assign point values(to point); 


list-»next node-»pt - assign point values(from point); 
return (list); 


My 


Mie OU A = 
This functions finds and returns the node which with the inputted point 
forms the plus common tangent. It calls the the order function. Sign is 


a macro and is defined in 3d tan.h and returns an integer corresponding 
to the value returned by order. The function returns the point found to 
the calling function*/ 


point plus tangent(start, pl) 
obstacle plane *start; 
point pl; 
[ 
obstacle plane *next = start->ccw, *prev = start->cw, *current = start; 
int count; 
point pt; 
double dirl, dir2; 
for (count = 1; count <= start->number nodes; count++) [ 


if (current->pt.x == pl.x && current->pt.y == pl.y) ( 


current = current->ccw; 
break; 
} 
if (sign(order(pl, current, next)) == 1) [ 
current = next; 
next = next->ccw; 
} 
else { 
if (sign(order(pl, current, prev)) -- 1) ( 
current - prev; 
prev - prev->cw; 
else 
break; 


J 
} 
if(atan2(pl.y - current->pt.y, pl.x - current->pt.x) == 
atan2(pl.y > current >2ccw >Pt.y, plex - current->ccw->pt.x))( 
if(distance(pl, current->pt) < distance(pl, current->ccw->pt)) 
return(current->pt); 
else 
return(current->ccw->pt); 


) 
meCatanZ(pl.y - current-»pt.y, pl.x - current-»pt.x) == 
atan2(pl.y - current->cw->pt.Y, pl.x - current->cw->pt.x))( 
if(distance(pl, current->pt) < distance(pl, current->cw->pt)) 
return(current->pt); 
else 
return(current->cw->pt); 
} 


return (current->pt); 
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SS _s 000 + O > LE 
finds and returns the node in the polygon which, with the inputted point 
forms the minus common tangent. It calls the the order function. Sign 


is a macro and is defined in 3d tan.h and returns an integer corresponding 
to the value returned by order. The function returns the point found to 
the calling function*/ 


point minus tangent(start, pl) 
obstacle plane *start; 
point pl; 


obstacle plane *next = start->cw, *prev = start->ccw, *current = start; 
int count; 
point pb. 


for (count = 1; count «- start-»number nodes; count**) { 


if (current->pt.x == pl.x && current->pt.y == pl.y) I 
current = current->cw; 
break; 


) 


if (sign(order(pl, current, next)) == -1) { 
prev = current; 
current = next; 
next = next->cw; 


else { 
if (sign(order(pl, current, prev)) == -1) { 
next = current; 
current = prev; 
prev = prev->ccw; 
} 
else 
break; 
] 
] 
if(atan2(pl.y - current->pt.y, pl.x - current->pt.x) == 
atan2(pl.y - current->ccw->pt.y, pl.x - current >ccw >pt.x< | 
if(distance(pl, current->pt) < distance(pl, current->ccw->pt)) 
return(current->pt); 
else 
return(current->cCcwŵ->pt 
] 
if(atan2(pl.y - current->pt.y, pl.x - current->pt.x) == 
atan2(pl.y - current->cw->pt.y, pl.x - current->cw->pt.x)) { 
if(distance(pl, current->pt) < distance(pl, current->cw->pt) ) 
FetCurn(CUrrent=2 DE). 
else 
return(current->cw->pt& 
] 


return (current->pt); 
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APPENDIX J 


This appendix contains the source code of vertical.c. This code corresponds to 
those functions used to find the vertical path over a polyhedron. 


include "3d_tan.h" 
include "plot.h" 


#define LOOP 


This function finds the appropriate vertical points formed by the inter- 
section of the vertical plane and each polyhedron. The function cycles 
through the polyhedron list checking each polyhedron and finding all 
intersection points for that polyhedron. It then determines the two 
highest points and checks checks if the intersection points are between 
the expanded point and the goal. If the points are, the function calls 
new_vertical new_list, which places one of the two or both of the points 
into a vertical list (link list). The function then checks the next 
polyhedron, finding the appropriate intersection points and calls the 
new vertical new list which replaces the points in the vertical list if 
they satisfy the requirments of the heuristics in that function. It 
recieves the vertical list from new vertical new list and passes it to 
the calling function upon completion.*/ 


vertical path *find vertical nodes(expand pt, end pt) 
point expand pt, end pt; /* expand pt is the point from 
* which the vertical list is 
* formed, end point is the 
* ending point that is used 
* to form the vertical list */ 


point ptl, pt2, temp; 

obstacle list  *current polyhedron; 

polyhedron obstacle *face; 

polyhedron obstacle plane *current node; 

THE count, boolean - FALSE, two points - FALSE; 
vertical path *vertical list - NULL; 


vertical n perpendicular plane(expand pt, end pt); 


for (current polyhedron - polyhedron list; 
current polyhedron !* NULL; 
current polyhedron - current polyhedron-»next polygon, 
two _ points = FALSE, boolean = FALSE) ( 


for (face = current polyhedron->obstacle; 
face !- NULL; face - face-»next face) I 


for (current node - face-»face nodes, count - 1; 
count «- face-»face nodes-»number nodes; 
currentinode current node->ccw, ceounttt) | 

if (two points == FALSE) [ 


if (boolean FALSE) ( 
ptl = intersection point(current node-»pt, 
current node-»ccw-»pt, vertical); 
if (ptl.x t= NO INTERSECTION) [ 
boolean = TRUE; 


continue; 
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] 
) 


else { 


pt2 = intersection point(current_node->pt, 
current_node>>ecw>>pt, vertical); 
if (pt2.x != NO_INTERSECTION) { 


two points = TRUE; 

if- (ptl 2T pPE2.Z) í 
temp = swap(ptl); 
ptl - swap(pt2); 
pt2 = swap(temp); 


} 
] 


else ( 
temp = intersection point(current_node->pt, 
current_node->ccw->pt, vertical); 
if (temp.x != NO_INTERSECTION) ( 


if (temp.x == ptl.x && temp.y == ptl.y && temp.z == ptl.z) ( 
if (temp.x == pt2.x && temp.y == pt2.y && temp.z == pt2.z) { 
continue; 
} 
) 


if (temp. 27 SDE Zo 
pt2 - swap(ptl); 
ptl - swap(temp); 


else { 
if (temp.z < pb27z) 
pt2 = swap(temp); 


} 
] 
) 


if (ptl.x !- NO INTERSECTION && boundry check(boundry,ptl) && 
boundry check(boundry, pt2) && 

(ptl.x !* end pt.x || ptl.y != end pt.y) && (ptl.x !- expand pt.x || 

ptl.y !- expand pt.y) && 

(pt2.x I— expand pe: li pt2.y != expand pt.y)) [{ /*the or checks are 
to ensure the expand or end point are not included as 
in the case of a face edge*/ 

if (vertical node direction heuristic(end pt, expand pt, ptl) && 
vertical node direction heuristic(end pt, expand pt, pt2)) 


vertical list - vertical node list fromipt(vertical list,ptl,pt2, 
expand pt, current polyhedron-»polyhedron number); 


) 
] 


return (vertical list); 


point swap(pt) 
point pi 

[ 
point pel; 
ptl.x = pt x; 
ptl.y = pt.y; 
ptl.z = pt.z; 


return (pti); 


136 


NEW VERTICAL NODE LIST FROM POINT 
This function uses the two vertical points passed to it from find verti- 
cal nodes, which represent the vertical nodes from a polyhedron, and 
places them in eucledian distance order in a vertical list. 


to the calling function this vertical list. 
El 


It returns 


vertical path *vertical node list from pt(vertical list,ptl,pt2, pt, poly num) 
vertical path *vertical list; 


point DENSMDt2, pt; 
int poly num; 
( 
double ptl distance, pt2 distance, pt dist; 
a path *current, *previous, *temp; 


pel distance e distance(ptl, pt); 
pt2 distance - distance(pt2, pt); 
NEW NODES(temp); 

temp-»next node - NULL; 


if(ptl distance -- pt2 distance)f( 
temp->pt - assign point values(ptl); 
temp->polygon number - poly_num; 


) 


if(ptl distance != pt2 distance) ( 

NEW NODES(temp-»next node); 

temp-»next node-»next node = NULL; 

if(ptl distance > pt2 distance)[ 
temp-»pt - assign point values(ptl); 
temp-»next node-»pt - assign point values(pt2); 
] 
else( 
temp-»pt - assign point values(pt2); 
temp-»next node-»pt - assign point values(ptl); 
] 
temp-»polygon number - poly num; 
temp-»next node-»polygon number - poly num; 


) 


if(vertical _ list == NULL) ( 
NEW VERTICAL(vertical list); 
vertical list >next node - temp, 


elsef{ 
for(current = vertical_list->next_nođe, previous = NULL; 
current. l= NULL || temp == NULL; 
previous = current->next_node, 
current = current->next_node->next_node) [ 


pt_dist = distance(pt, current->pt); 
if(ptl distance » pt dist || pt2 distance > pt dist)[ 
temp->next_node->next_node = current; 
if(previous == NULL) { 
vertical list-»next node - temp; 
break; 
] 
else 
previous-»next node - temp; 
] 
if(current-»next node -- NULL)( /*considers only one node in list 


and temp nodes come after it*/ 
current-»next node - temp; 


break; 


) 


if (temp !- NULL && previous !- NULL) 


By, 


previous->next_node = temp; 
J 
vertical list->next_node->next_after_node = NULL; 
return(vertical list); 


This function takes the vertical list and inserts the list as a valid 

path in the start_paths list. 

ay 

paths *insert_verticle list(start_paths, current path, vertical list, tan4verc, 
end pt, polygon4vert) 


paths *start_paths, *current_path; 
vertical path *vertical list; 
int tan4vert; 


point end pt; 


paths *temp; 
a path *current; 
double cost = 0; 


NEW PATH(temp); 

temp-»last node - vertical list-»next node; 
current - temp-»last node; 
current-»next after node = NULL; 

temp-»last node-»next after node = NULL; 
free(vertical list); 


/*connect the double link list and calculate the distance from the 
first node to the last node*/ 


for( ; LOOP ; current - current->next node)[ 
if(current->next node !- NULL)[ 
current->next node->next arLemn mode Neue rene 


cost += distance(current->pt, current->next_node->pt); 
) 
else 
break; 
) 


temp->polygon4vert = polygon4vert; 

temp->polygon4vert_PTR = NULL; 

temp->cost = cost; 

temp->estimated_ cost = distance(temp->last_node->pt, goal); 

temp->total_cost = cost + temp->estimated cost; 

temp->type tangent = VERTICAL; 

temp->ray direction - atan2((temp->last node->pt.y - 
temp->last_node->next_node->pt.y), 
(temp->last_node->pt.x - 
temp->last_node->next_node->pt.x)); 

temp->plane number = 0; 

temp-»when path found - paths found**; 

temp-»able2expand = TRUE; 

temp->polygon4vert_PTR = input _data4polyhed4vert(temp->polygon4vert_PTR, 

end_pt, tan4vert); 


temp-»next path - current path-»next path; 


if(start paths -- NULL) 
start paths - temp; 
else[ 


temp->next_path - current_path->next path; 
current_path->next_path = temp; 


) 
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Hifdef DOS 
if (plot2screen) { 
for(current = temp->last_node; 
current->next_node != NULL; 
current scurrenbesemext node) 
draw line segment(current-»next node-»pt, current-»pt, YELLOW, 
REGULAR); 


] 
Hendif 


return (start paths); 


This function determines if a point lies in the same xy direction from a 
start point as the direction of the line from the start pt to the goal. 
The start pt is not the same as the global start point but just a local 
variable and could any valid point in the valid possible path list 

EZ 


int vertical_node_direction_heuristic(goal, expand_pt, pt) 
point goal, expand_pt, pt; 

( 
double X, Y, a; 
if(goal.x == expand_pt.x || pt. x95. expand pt.x) 


expand pt.x += .0000001; 
if(goal.x == pt.x) 


goal - increase goal x(goal, expand pt); 
x = atan2(goal.y - expand pt.y, goal.x - expand pt.x); 
y = atan2(goal.y - pt.y, goal.x - pt.x); 
a =X- y; 


if (ABS(a) <= OPI) 
return (1); 
else 
return (0); 


/*---------------------------------------------------------------------- 
INCREASE GOAL X 

EMEN ul s A Mr. h.h... 

This function is called if its determined that the increase and pt2 points 

have the same value for the x coordinate. if so then it adds of subtracts 

.000001 to the x component goal so that the new distance from the two 

points is greater than the original distance. If not done, atan2 is 

undetermined since x - X == 

A 


point increase goal x(increase, pt2) 
point increase, pt2; 
[ 


double orginal distance; 

orginal distance = distance(increase, pt2); 
increase.x += .0000001; 

if(distance(increase, pt2) < orginal distance) 


increase.x -= .0000002; 


return(increase); 
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This function calls two other functions. The first finds the verti- 
cal nodes of a possible path and places them in a list labled verti- 
cal list. The second function takes the vertical list, places it 

in the possible paths list and updates the header list of the path. 
"A 


paths *up and over(start paths, current path, exp pt, end pt, beginpt, mode, 
polygon4vert) 


paths *start paths, *current path; 
point exp pt, end pt, beginpt; 
int mode, polygon4vert; 


vertical path  *vertical list - NULL; 
a path *current; 


vertical list - find vertical nodes(exp pt, end pt); 
if (vertical list != NULL) ( 
if(beginpt.x == exp _pt.x ££ beginpt.y == exp pt.y && 
beginpt.z -- exp pt.z)[ 
for(current - vertical list-»next node; 
current-»next node !- NULL; 
current - current-»next node); 


NEW NODES(current-»next node); 


current-»next node-»next node = NULL; 
current-»next node-»pt - assign point values(exp pt); 


vertical list - trim not needed nodes(vertical list); 


start paths - insert verticle list(start paths, current path, 
vertical list, mode, end pt, 
polygon4vert); 
] 
if(VERTICAL t= mode)(] 


return(start paths); 


This function finds the one node/tangent which can extend the verti- 
cal path by one node on the perpendicular plane formed by the verti- 
cal node and the goal. There is only one node to consider since it 


falls on the line segment of the polyhedron used to find the verti- 
node 
tr 
paths *continue_vertical path(shortest, start2d) 
paths *shortest; 
polygon list *start2d; 


polygon list *polygon - start2d; 
point pt1, pt2; 

a_path *temp; 

int tan,found = FALSE; 

double ray_direction, difference; 


for (; polygon != NULL; polygon - polygon->next polygon)[ 
if(polygon->plane_ number !- shortest->plane number) 
continue; 
if(polygon->polyhedron number !- shortest->polygon4vert) 
continue; 
else 
break; 
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if(1 == shortest->polygon4vert_PTR->type tangent) 

ptl - plus tangent(polygon-»polygon start, shortest-»last node->pt); 
else 

ptl - minus tangent(polygon-»polygon start, shortest-»last node-»pt); 


ray direction = atan2(ptl.y - shortest->last_node->pt.y, 
ptl.x - shortest->last_node->pt.x); 


/*used le-3 instead of 0 because of the precision of DOS machine. No 
points used evaluated true and the point which should have evalueated 
true, had a difference of le-10*/ 


difference = shortest->ray direction - ray direction; 
/*find the up around and up paths before continuing on*/ 


if(ABS(difference) < le-3)( 
NEW_NODES (temp) ; 
temp->pt = assign point_values(ptl); 
temp->polygon_number = polygon->polygon_number; 
temp->next_node = shortest->last_node; 
shortest->last_node = temp; 
if(shortest->polygon4vert PTR->type tangent) tan = PLUS; 
else tan = MINUS; 
shortest = update(shortest, tan); 
found = TRUE; 


else( 
shortest - recalculate vertical path(ptl, shortest, 
(shortest->polygon4vert PTR->type tangent ?PLUS: 
MINUS)); 


shortest->last_node->polygon number - polygon->polygon_ number; 
found - TRUE; 
] 


if (found) 
shortest->able2expand - NO; 


Hifdef DOS 
else[ 
if (plot2screen) 
draw line segment(shortest-»last node-»next node-»pt, 
shortest-»last node-»pt, YELLOW, REGULAR); 


] 
#endif 


return(shortest); 


This function finds all the vertical partial paths from the goal 
extending to the start. This working backwards from the goal allows 
me to find the valid paths which go around one or more obstacles and 
over the rest of then. 
A 
paths *up and over from goal(paths *start paths, paths *current path, 
point goal, polygon list *start2d) 

/* paths *start paths, *current path; 

point goal; 

polygon list *start2d; 

t 


polygon list *current polygon; 
point ptl, pt2; 
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] 


int poly4pt; 


for (current polygon - start 2d; 
current polygon !- NULL; 
current polygon - current polygon-»next polygon) [ 


ptl 
pt2 


plus_tangent(current_polygon->polygon start, goal); 
minus tangent(current polygon-»polygon start, goal); 


if (cross polygon(start 2d, goal, ptl) && 
direction heuristic from start(goal, ptl) && 
boundry check(boundry, pt1)) I 


start paths - up and over(start paths, current path, goal, 
ptl, goal, PLUS, current polygon->polyhedron number); 
if (current path == NULL) current_path = start_paths; 
else current path - current path-»next path; 
current path-»forward = NO; 
] 
if (cross polygon(start 2d, goal, pt2) && 
direction heuristic from start(goal, pt2) && 
boundry check(boundry, pt2)) í 


start paths - up and over(start paths, current path, goal, 
pt2, goal, MINUS); 
if (current path -- NULL) current path - start paths; 
else current path - current path-»^next path; 
current path-»forward - NO; 
current path-»polygon4vert - current polygon-»polyhedron number; 


] 


return(start paths); 


paths *recalculate vertical path(pt, path, tan) 


point pt; 
paths *path; 
int tan; 


vertical path *vertical; 

a path *node - path-»last node, 
*current, *previous; 

double cost = 0; 

int count, number nodes; 


/*find the last node (or beginning pt of path)*/ 
for( ;node-»next node !- NULL; node - node-»next node); 


/*disconnect the last node from the path*/ 
node-»next after node-»next node - NULL; 
node-»next after node = NULL; 


/*delete the path nodes of the shortest path 
while counting number of nodes*/ 


for(current - path-»1last node, previous = current, number nodes = 0; 
current !- NULL; 
previous = current, current- current-»next node, number nodes-*-*) 


free(previous); 
free(previous); 


/*find the new vertical list*/ 
vertical - find vertical nodes(node->pt, pt); 


/*finds the end of the vertical list then working backwards up the 
list, places the same number of nodes in the list*/ 
for(current - vertical-»next node; current-»next node !- NULL; 
current current->next_node) 
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] 


current-»next node-»next after node = current; 


for(count - 1; count « number nodes; 
count**, current- current-»next after node); 
vertical-»next node - current; 
/*frees up the unused portion of the list*/ 
for(current - current-»next after node; current !- NULL; 
current current-»next after node) 


free(current); 


path-»last node - vertical-»next node; 
free(vertical); 

path = insert pt into list(path, pt); 
path-»last node-»next after node - NULL; 


for(current - path-»last node; 
current-»next node !- NULL; 
current = current~>next_node) [ 
current->next_node->next_after_node = current; 
cost += distance(current->pt, current->next_node->pt); 
] 
current-»next node - node; 
node-»next after node - current; 
cost += distance(current->pt, node->pt); 
if(path->forward == YES) 
path-»estimated cost = distance(node->pt, goal); 
else 
path->estimated cost - distance(node->pt, start point); 


path-»total cost - cost * path-»estimated cost; 
path->cost = cost; 
path->ray direction = atan2(path->last_node->pt.y - 
path->last_node->next_node->pt.y, 
path->last_node->pt.x - 
path->last_node->next_node->pt.x); 
path-»type tangent - tan; 


return(path); 


paths *insert pt into list(path, pt) 


] 


paths *path; 
pornt pt; 


a path *temp - NEW NODES(temp); 
temp-»pt - assign point values(pt); 
temp-»next after node - NULL; 
temp-»next node path-»last node; 
path-»last node temp; 


return(path); 


polyhed4 *input data4polyhed4vert(ptr, end pt, mode) 


polyhed4 *ptr; 
point end pt; 
int mode; 


if(NULL == ptr) 
ptr = (polyhed4 *) malloc(sizeof(polyhed4)); 


ptr-»x - endEept x; 
ptr->y = end pt.y; 
ptr->type tangent = mode; 


return(ptr); 
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vertical path *trim not needed nodes(list) 
vertical path *list; 
( 


a path *nodel, *node2, *node3, *node4; 


for(nodel - list-»next node; 
NULL != nodel; 
nodel = nodel->next_node) [{ 
if(NULL == nodel->next_node->next_node) break; 
for(node2 = nodel->next_node->next_node; 
NULL !- node2; 
node2 = node2->next_node) [{ 


if(intersect polyhedron(nodel->pt, node2->pt)) continue; 


else[ 
for(node3 - nodel-»next node, node4 - node3; 
node2 !- node3; 
node4 - node3, node3 - node3-»next node,free(node4)); 
nodel-»next node - node2; 
node2-»next after node - nodel; 


J 
] 
] 


return(list); 
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APPENDIX K 


This appendix contains the source code associated with visibile.c. 


Hinclude "3d tan.h" 
include "plot.h" 


define LOOPFOREVER 1 


This function determines whether or no the last point in the partial path is 
visible to the goal or not. The function considers whether a point on the 
backside of a polygon is visible to the goal by first finding the tangents to 
the goal with the polygon and traces the path around to the expansion point. 
A point is valid in this case if no obstacles lie between the goal and the 
tangent point 
E 
a path *visibility(expand point, goal point, start, two d list) 

point expand point, goal point, start; 

polygon list *two d list; 


polygon list *polygon; 

point from polygon pt, goal; 

a path *temp, *templ, *goal list; 
obstacle plane *node; 

int tangent = l; 


goal = determine_end_point(start, goal_point); 
if(!intersect polyhedron(expand point, goal))( 
NEW NODES(goal list); 
goal list->next_node - NULL; 
goal list-»pt - assign point values(goal); 
return(goal list); 


J 
if(shortest_path->type tangent == VERTICAL) 
return(NULL); 
elsef 
/*find polygon which last node of shortest path lies on*/ 
for(polygon - two d list; 
polygon-»polygon number !- shortest path-»last node-»polygon number || 
polygon-»plane number !- shortest path-»plane number; 
polygon - polygon->next polygon); 


while(tangent <- 2)( 
if(l == tangent) 
/*finds the plus tangent from the goal to the polygon last node of 
shortest path lies on*/ 
from_polygon_pt = plus tangent(polygon->polygon_start, goal); 
else 
/*check the minus tangent*/ 
from polygon pt - minus tangent(polygon-»polygon start, goal); 


/*checks for intersection, if there is return a zero*/ 
if(intersect polyhedron(from polygon pt, goal)) 


return(NULL); 
else[ /*two points are visible*/ 
if(temp == NULL) f 


NEW NODES (temp) ; 
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temp-»next node - NULL; 

temp-»next after node - NULL; 
] 
temp->pt - assign point values(from polygon pt); 
templ - temp; 


/*finds the node on the polygon which the from polygon pt lies on*/ 
for (node = polygon->polygon start; 


node->pt.x != from polygon_pt.x 
node-»pt.y !- from polygon pt.y 
node-»pt.z !- from polygon pt.z; 
node - node->ccw); 


while (LOOPFOREVER) [ 
if(1 == tangent)Í 
/*checks to see if the next counter clockwise node is the 
expansion node*/ 
if(node->ccw->pt.x 
node->ccw->pt.y 
node->ccw->pt.z 
break; 
elsef 
if(templ-»next node == NULL) |[( 
NEW NODES(tempi-»next node); 
tempi-»next node-»next after node - tempi; 
templ-»next node-»next node - NULL; 
] 
templ - templ-»next node; 
templ-»pt - assign point _values(node->ccw->pt); 


shortest path-»1ast node-»pt.x && 
shortest path-»1ast node-»pt.y && 
shortest path-»last node-»pt.z) 


low M 
ll HON 


) 


node = node->ccw; 


) 
else[ /*tangent == 2*/ 
/*checks to see if the next counter clockwise node is the 
expansion node*/ 


if(node-»cw-»pt.x -- shortest path-»1ast node-»pt.x && 
node-»cw-»pt.y -- shortest path-»last node-»pt.y && 
node-»cw-»pt.z -- shortest path-»1last node-»?pt.z)Í 
if(templ-»next node !- NULL) 


for(templ - templ->next_ node, 
templ->next after node->next node = NULL, 
templ-»next after node - NULL, 
templ-»next node; 
templi-»next node | NULL; 
templ = templ-»next node) 
free(templ-»next after node); 


if(templ-»next after node !- NULL) 
free(templ-»next after node); 

free(templ); 

) 

for(templ - temp; temp-»next node !- NULL; temp - temp-»next node); 
break; 
else[ 

if(templil-»next node -- NULL)[ 

NEW NODES(templ-»next node); 

templ->next node->next after node = templ; 


templ-»next node-»next node - NULL; 


) 
templ - templ-»next node; 
templ-»pt - assign point values(node-»cw-»?pt); 


node = node->cw; 
} 
) 
if (direction heuristic(shortest path, tempi-»pt))[ 


goal list - temp; 
return(goal list); 
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] 
] 
tangent *- 1; 


] 


for(templ - temp-»next node; templ-»next node != NULL; templ - templ-»next node) 
free(templ-»next after node); E 

free(temp-»next after node); 

free(temp); 

return(NULL); 


point determine end point(start, goal) 
point start, goal; 


( 
if(shortest path-»forward) return(goal); 
else return(start); 
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APPENDIX L 


This appendix contains the two header files (3d tan.h and plot.h). 


#include <math.h> 
#include <stdio.h> 


#define 
#define 
#define 
#define 
#define 
#define 


READONLY 
WRITEONLY 


TRUE 
FALSE 
NO 
YES 


“er A O0 0% === ==“ O LLL 

Nr" 

"o" 

1 

0 

0 

1 

N 99999979990 


#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 


#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 


#define 


NO_INTERSECTIO 


sqr(x) 


E(X Y 2) 
VERTICAL 


NONE 
PLUS 
MINUS 


GOAL PT 
NOT GOAL PT 


sign(a) 
ABS(x) 


TOTAL COST 


FOREVER 
QOPI 

HPI 

PI 

DPI 


NEW_OBSTACLE(X) 
NEW_PLANE(x) 


NEW NODE(x) 


(x * xX) 


((pl eg.x*temp pt.x) * (pl eg.2*temnp pt.2))/pYEcO 
2 
2 
1 
0 
1 
0 
(((a) < 0) ? -1l (a) > 0 ? 1 : 0) 
((Cx) €90) 7?) =x (x) 20x 05 
current path-»cost * current path-»estimated cost 
1 
0.785398230785398 
135707963257 949 
3.141593) 23741533 
6.283186246283186 
((x = (obstacle *) malloc 
(sizeof (obstacle)))--NULL?exit(1):1) 
((x = (obstacle plane *) malloc 
(sizeof (obstacle plane )))#==NULL?exit(1):1) 
((x = (obstacle list *) malloc 
(sizeof(obstacle list))) == NULL?exit(1):1) 


NEW POLYHEDRON(x) ((x = (polyhedron obstacle *) malloc 


(sizeof (polyhedron obstacle))) == NULL?exit(1):1) 


NEW POLY PLANE(x) ((x = (polyhedron obstacle plane *) malloc 


NEW VERTICAL(X) 
NEW GOAL(X) 

NEW PATH(X) 

NEW NODES(x) 


NEW POLYGON(x) 


(sizeof(polyhedron obstacle plane))) -- NULL?exit(1):1) 
((x = (vertical path *) malloc 


(sizeof(vertical path))) == NULL?exit(1):1) 
((x = (goals *) malloc 
(sizeof(goals))) == NULL?exit(1): 1) 
((x = (paths *) malloc 
(sizeof(paths))) == NULL?exit(1): 1) 
((x = (a path *) malloc 
(sizeof(a path))) == NULL?exit(1):1) 
((x * (polygon list *) malloc 
(sizeof(polygon list))) -- NULL?exit(1):1) 


NEW ACTIVE NODE(x) ((x - (active nodes *) malloc 


POINT VALUES 


(sizeof(active nodes))) -- NULL?exit(1):1) 
&x coord, &y coord, &z coord 
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#ifdef MAIN 

# define EXTERN 

# define INIT(Value) = Value 
#else 

# define EXTERN extern 

# define INIT(Value) 

#endif 


typedef struct polyhedron list f 
struct polyhedron *obstacle; 


int number faces; 

int polyhedron number; 

struct polyhedron list *next polygon; 
] obstacle list; 


EXTERN obstacle list *polyhedron list INIT(NULL); 
EXTERN obstacle list *intersected polyhedron INIT(NULL); 


typedef struct polyhedron [ 
struct polyhedron node *face nodes; 


int number nodes; 
Struct polyhedron *next face; 
) polyhedron obstacle, polyhedron face; 
typedef struct posture ( 
double X; 
double y; 
double De 
double phi; 
double i 
double psi; 
] POSTURE; 
typedef struct x y z [ 
double xj 
double y; 
double Z; 
} point; 
typedef struct polyhedron node [ 
SEBUCE Xx y z pty 
int number nodes; 


struct polyhedron node *cw; 
struct polyhedron node *ccw; 
] polyhedron obstacle plane, polyhedron nodes, obstacle plane; 


EXTERN point start point, goal, intersection; 
typedef struct plane equation [ 

double Xx: 

double y; 

double Ze 

double d; 
) eguation; 


EXTERN eguation vertical, perpendicular; 


typedef struct two d polygon list [ 
obstacle plane *polygon start; 
Struct two d polygon list *next polygon; 


int plane number; 

int polygon number; 

int polyhedron_number; 
} polygon_list; 


EXTERN polygon_list *start_2d INIT(NULL); 
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/*typedef struct polygon node [ 
struct x y z pt; 
int number_nodes; 
struct polygon_node *ccw; 
struct polygon_node *cw; 


] obstacle plane;*/ 
typedef struct list of active nodes [ 
point pt; 
double distance; 
struct list_of_active_nodes *next_active_node; 
int when_path_found; 
] active nodes; 


EXTERN active nodes *head active nodes INIT(NULL); 


typedef struct list of paths [ 
struct list of paths *next path; 
struct possible paths *last node; 


double cost; 

double estimated cost; 

double total cost; 

double ray aiirection; 

int type _ tangent; 

int plane number; 

int when_path_found; 

int able2expand; 

int forward; 

int polygon4vert; 

struct polyhed  *polygon4vert PTR; 
) paths; 


EXTERN path and node list *head path node list INIT(NULL); 
EXTERN paths *start path INIT(NULL); 
EXTERN paths *shortest path INIT(NULL); 


typedef struct possible paths [ 
point pe, 
struct possible paths *next_node; 
struct possible paths *next after node; 
int polygon number; 

) a_path, goals; 


typedef struct vertical[ 

double distance; 

struct possible paths *next_node; 
] vertical par, 
EXTERN goals *goal path INIT(NULL); 


typedef struct polyhed{ 


double x, 
y; 
int type tangent; 
)polyhed4; 
typedef struct boundry limits { 
double xl; 
double x2; 
double yb 
double Ya; 
double zi 
double 225 
) boundries; 


EXTERN boundries boundry; 
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EXTERN char Tile[ll]; 

EXTERN char *filename INIT(file); 
EXTERN int paths found INIT(0); 
EXTERN int plane number INIT(0); 
EXTERN int print2file INIT(0); 

EXTERN int horizontal INIT(0); 
EXTERN int plot2screen INIT(0); 
EXTERN int on same polygon INIT(0); 
EXTERN int number polygons; 
EXTERN int number paths found INIT(0); 
EXTERN double goal direction INIT(0); 
EXTERN FILE *fpt2 INIT(NULL); 


/* function declarations */ 
extern active nodes *insert nodes into active node list(); 


extern a path *free tangent list(); 
extern a path *make tangent list(); 
extern a path *tangent(); 

extern a path *visibility(); 


extern boundries assign boundry points(); 


extern double function_one(); 
extern double function_two(); 
extern double norm(); 
extern double order(); 


extern int boundry_check(); 

extern int check active node list(); 

ExPern int check for node on polygon(); 
extern int cross polygon(); 

extern int cross polygon node on(); 

extern int count polygon nodes(); 

extern int decision(); 

extern int intersect polyhedron(); 

extern int lines intersect(); 

extern int point on line(); 

Extern int vertical node direction heuristic(); 
extern int direction heuristic from start(); 
extern int direction heuristic(); 


extern equation cross product(); 
extern equation find plane equation(); 


extern double calculate cost(); 
extern double distance(); 


char *malloc(); 
extern obstacle list *create obstacle list(); 
extern obstacle plane *create list(); 


extern paths *add start point to path(); 

extern paths *create path n nodes(); 

extern paths *connect links of path on polygon(); 
extern paths *connect shortest paths(); 

extern paths *continue vertical path(); 

extern paths *delete before final(); 


extern paths *duplicate entries(); 

extern paths *duplicate paths(); 

extern paths *expand paths with values(); 
extern paths žextend_path(); 

extern paths *find tangents from inner node(); 


extern paths *goal reached update(); 
extern paths *insert pt into list(); 
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extern paths *insert_verticle list(); 


extern paths *insert vertical list into shortest path(); 
extern paths *mark old path with node unexpandable(); 
extern paths *mark shortest path last node(); 

extern paths *up and over(); 

extern paths *up and over from goal(); 

extern paths *up around up(); 

extern paths *recalculate vertical path(); 

extern paths *remove shortest path(); 

extern paths *remove paths(); 

extern paths *reversed list(); 


extern paths *start finding paths(); 
extern paths *update current path(); 
extern paths *update(); 


extern point assign point values(); 

extern point determine end point(); 

extern point find shortest patn(); 

extern point increase goal x(); 

extern point input point(); 

extern point intersection point(); 

extern point plus tangent(); 

extern point minus tangent(); 

extern point find point on obstacle plane face(); 
extern point swap(); 


extern polygon list *add on new plane intersection(); 
extern polygon list *build polygon(); 

extern polygon list *build two d polygon list(); 
extern polygon list *connect links(); 

extern polygon list *free polygon list(); 

extern polygon list *make listos 

extern polygon list *remove duplicate nodes(); 

extern polygon list *remove last polygon(); 

extern polygon list *remove planes(); 


extern polyhedron face *find polyhedron face(); 


extern polyhedron obstacle *create face(); 
extern polyhedron obstacle plane *create plane(); 


extern polyhed4 *input data4polyhed4vert(); 


extern void read comment(); 

extern void find all paths(); 
extern void free memory(); 

extern void input start goal(); 
extern void inside boundries(); 
extern void load goal path(); 
extern void vertical n perpendicular plane(); 
extern void print intersection(); 
extern void print results(); 
extern void print 2d path(); 
extern void print each paths(); 
extern void print path(); 

extern void print path shortest(); 
extern void print polyhedron(); 
extern void print path to file(); 


extern vertical path *find vertical nodes(); 

extern vertical path *new vertical new list(); 
extern vertical path *trim not needed nodes(); 
extern vertical path *vertical node list from pt(); 
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#include <graphics.h> 
#include <dos.h> 


#define DOS il 

#define ESC Oxlb/* Define the escape key*/ 

#define SCALEDX x / 48 

define SCALEDY y / 34 

#define DOT 1 /*used to make a dot with radius l*/ 
define RED 12 

define YELLOW 14 

define REGULAR 0 


#ifdef MAIN 

# define EXTERN 

# define INIT(Value) = Value 
#else 

# define EXTERN extern 

# define INIT(Value) 


#endif 

EXTERN int GraphDriver INIT(0);/* The Graphics device driver*/ 

EXTERN int GraphMode INIT(0);/* The Graphics mode value*/ 

EXTERN int MaxX, MaxY INIT(0);/* The maximum resolution of the screen */ 
EXTERN int MaxColors INIT(0);/* The maximum # of colors available*/ 
EXTERN int ErrorCode INIT(0);/* Reports any graphics errors*/ 


EXTERN double AspectRatio INIT(0.0);/* Aspect ratio of a pixel on the screen*/ 
EXTERN struct palettetype palette;/* Used to read palette info*/ 

EXTERN int Expansion lable x INIT(0);/* Used to label the expansion point*/ 
EXTERN int Expansion lable y INIT(0); 


extern void data plot(); 

extern void Initialize(); 

extern void Pause(); 

extern void MainWindow(); 

extern void DrawBorder(); 

extern void StatusLine(); 

extern void changetextstyle(); 
extern void draw boundry(); 
extern void draw polygons(); 
extern void draw shortest path(); 


extern void draw line segment(); 
extern void id shortest path for expansion(); 
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APPENDIX M 


This first example is for 1 polyhedron and corresponds to the first example in 
Chapter IV. 


INITIAL PARTIAL PATHS 


PATH 1 
PATH NUMBER O Path able to extend 


cost - 11.589, est.cost - 31.192, total - 42.781 
Plus Tangent, direction = 0.661 

(10.00, 20.00, 7.000 2o 

(1. 00 13700, 5-003 

PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 11.014, est.cost = 31.811, total = 42.825 
Minus Tangent, direction = -0.588 

(10.00, 7.00, 7.08) Go 

(100, 13500 75 00) 

PATH 3 

PATH NUMBER 2 Path able to extend 

cost - 13.236, est.cost - 28.263, total - 41.499 
Minus Tangent, direction = 0.051 

(14.00, 13.67/43%00) to 

(10.00, 13.46, 3.00) to 

(1.00, 13.00, 5.00) 

THE GOAL HAS BEEN REACHED 

PATH FINDING ITERATION COUNT = 2 

PATH 1 

PATH NUMBER 0 Path able to extend 

cost = 11.589, est.cost = 31.192, total = 42.781 
Plus Tangent, direction = 0.661 

(10.007 20.00, 7.08) to 

(1:00 213:00, 5200) 

PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 11.014, est.cost = 31.811, total - 42.825 
Minus Tangent, direction = -0.588 

(10.00, 7.00, 7.08) to 

(LADO 13200, 5 00) 

PATH 3 

PATH NUMBER 2 Path able to extend 

cost - 13.236, est.cost - 28.263, total - 41.499 


Minus Tangent, direction - 0.051 
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(14.00, 
(10.00, 13.46, 
(1.00, 13.00, 
SHORTEST PATH 


13-67, 4395095 to 
3.00) to 


5.00) 


(14.00, 13.67, 3.00) to 
(10.00, 13.46, 3.00) to 
(00, 13.00, 5.00) 


The total length of the shortest path is 41.499 
PATH FINDING ITERATION COUNT = 3 
PATH 1 


PATH NUMBER O0 
cost = 11.589, 


Path able to extend 
est.cost = 31.192, 


total 42.781 


Plus Tangent, direction = 0.661 
ROO, 20.00, 7.08) to 
00, 13.00, 5.00) 


PATH 2 
PATH NUMBER 1 
cost = 11.014, 


Path able to extend 
est.cost = 31.811, 


total 42.829 


Minus Tangent, direction = -0.588 
œo O00, 7.00, 7.08) to 
ADO, 13.00, 5.00) 


PATH 3 
PATH NUMBER 2 
cost = 41.499, 


Path able to extend 
est.cost = 0.000, total = 41.499 
direction = 0.051 
14.00) to 

14:00) to 

3500) ^to 

3.00) to 


5.00) 


Minus Tangent, 
(0:00, 15.00, 
(30700, 15.00, 
i700, 13.67, 
(10.00, 13.46, 
(00, 13.00, 

SHORTEST PATH 


(40.00, 
(40.00, 
(4.00, 


15.00, 
15:00, 
13.67, 


14.00) to 

14 .00)&o 

3.00)" to 

OOO, 13.46, 3.00) to 

meou, 13.00, 5.00) 

The total length of the shortest path is 41.499 


This example is the listing generated by the algorithm for the second example for 
a one polyhedral world in Chapter IV. 


INITIAL PARTIAL PATHS 


PATH 1 

PATH NUMBER O Path able to extend 

cost = 11.589, est.cost = 31.192, total = 42.781 
Plus Tangent, direction = 0.661 

(10.00, 20.00, 7.08) to 

00, 13.00, 5.00) 

PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 11.014, est.cost - 31.811, total = 42.825 
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Minus Tangent, direction = -0.588 
(10.00, 7.00, 7/-0E We 
(1.00, 13:00, 5:06) 


PATH 3 
PATH NUMBER 2 Path able to extend 
cost = 13.236, est.cost = 28.263, total - 41.499 


Minus Tangent, direction = 0.051 
(14.00, 13.67, 3.00) to 

(10.00, 13.46, 3.00) Co 

(1.00, 13.00, 35-003 

THE GOAL HAS BEEN REACHED 

PATH FINDING ITERATION COUNT = 2 


PATH 1 
PATH NUMBER O Path able to extend 


cost - 11.589, est.cost - 31.192, total - 42.781 
Plus Tangent, direction - 0.661 

(10.00, 20.00, 7.08) to 

(1.00, 13.00, 5.00) 

PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 11.014, est.cost = 31.811, total = 42.825 
Minus Tangent, direction = -0.588 

(10.00, 7.00, 725089950 

(1.00; 13.00, 5.00) 

PATH 3 

PATH NUMBER 2 Path able to extend 

cost = 13.236, est.cost = 28.263, total = 41.499 


Minus Tangent, direction = 0.051 
(141.005, 515.67, 13900 9/900 

(10.00 13.40, 3 00 Go 

(1.00; T3.00,; 5.00) 

SHORTEST PATH 


(14.00, 13.67, 3:00) to 

(10.00, 13.46, 3.00) to 

(1.00, 13.00, 5.00) 

The total length of the shortest path is 41.499 


PATH FINDING ITERATION COUNT = 3 


PATH 1 
PATH NUMBER O Path able to extend 


cost = 11.589, est.cost = 31.192, total = 42.781 
Plus Tangent, direction = 0.661 

(10200, 20.00, 7:08) to 

(100 901535 500.95 500) 

PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 11.014, est.cost - 31.811, total - 42.825 
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Minus Tangent, direction = -0.588 
MEDEDO, 7.00, 7.08) to 
fio, 13.00, 5.00) 


PATH 3 
PATH NUMBER 2 Path able to extend 
cost = 41.499, est.cost = 0.000, total = 41.499 


Minus Tangent, direction = 0.051 
(40.00, 15.00, 14.00) to 

(40.00, 15.00, 14.00) to 

4900, 13.67, 3.00) to 

(10.00, 13.46, 3.00) to 

81:005, 13.00, 5.00) 

SHORTEST PATH 


(40.00, 15.00, 14.00) to 

(40.00, 15.00, 14.00) to 

"1 00, 13.67, 3.00) to 

(10.00, 13.46, 3.00) to 

00, 13.00, 5.00) 

The total length of the shortest path is 41.499 


This next example corresponds to the first example used in a two polyhedral world 
in Chapter IV. 


INITIAL PARTIAL PATHS 


PATH 1 

PATH NUMBER O Path able to extend 

cost - 10.296, est.cost - 30.414, total - 40.709 
Plus Tangent, direction - 0.507 

(10.00, 20.00, 14.00) to 

(1.00, 15.00, 14.00) 

PATH 2 

PATH NUMBER 1 Path able to extend 

cost - 12.042, est.cost - 31.048, total - 43.090 


Minus Tangent, direction - -0.727 
(10.00, 7.00, 14.00) to 
(“rm 00, 15.00, 14.00) 


PATH 3 
PATH NUMBER 2 Path able to extend 
cost - 18.545, est.cost - 28.438, total - 46.983 


Minus Tangent, direction - 0.257 
(14.00, 18.42, 3.00) to 

(19000, 17.37, 3.00) to 

MODO 5.00, 14,00) 


PATH 4 
PATH NUMBER 3 Path able to extend 


cost = 19.049, est.cost = 28.757, total 47.806 


Minus Tangent, direction = -0.399 
(141500, 9.53, 3.00) to 

(1000, 11.21, 3.00) to 

(1200, 15.00, 14-00) 


1S7 


PATH 5 
PATH NUMBER 4 Path able to extend 
cost = 28.213, est.cost = 19.416, total = 47.629 


Minus Tangent, direction = 0.000 
(24.00; 15.00, 3.00 Ee 

(20500, 15.00, 3.00 )WEOo 

(14700, 1500, 3100 EW 

(10.00, 15.00, 3:00 Mus 

(1200, 15300, 14 3005 


PATH 6 
PATH NUMBER 5 Path able to extend 
cost = 24.216, est.cost = 23.640, total = 47.856 


Minus Tangent, direction = -2.843 
(20,00, 8.85, 3.00) T tO 

(24.00, 10.08, 3-00) EGO 

(40.00, 15.00, 14.00) 


PATH 7 
PATH NUMBER 6 Path able to extend 
cost = 23.732, est.cost = 23.147, total = 46.879 


Minus Tangent, direction = 2.952 
(20.007 1821285,3300) tO 

(24500, 16208, 3.200) to 

(40500, 15.00, 14700) 

SHORTEST PATH 


(10.00, 20.00, 14.00) to 

(1200, 15100, 14.00) 

The total length of the shortest path is 40.709 
THE GOAL HAS BEEN REACHED 

PATH FINDING ITERATION COUNT = 2 


PATH 1 
PATH NUMBER O Path cannot be extended 


cost = 10.296, est.cost = 30.414, total - 40.709 
Plus Tangent, direction = 0.507 

(102500, 20:00, 14.00) to 

(1.00, 15.00, 14.00) 

PATH 2 

PATH NUMBER 10 Path able to extend 

cost = 28.613, est.cost = 21.541, total = 50.154 


Minus Tangent, direction = -1.138 
(20-00, 7400, 14.00) to 

(14.00, 20.00, 14.00) to 

(10.00, 20.00, 14.00) to 

(1.00, 15.00, 14.00) 


PATH 3 
PATH NUMBER 8 Path able to extend 
cost = 20.296, est.cost = 20.616, total = 40.911 


Plus Tangent, direction = 0.000 


(20.00, 20.00, 14.00) to 
(14.00, 20.00, 14.00) to 
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(10.00, 20.00, 14.00) to 
(1.00, 15.00, 14.00) 


PATH 4 

PATH NUMBER 1 Path able to extend 

cost = 12.042, est.cost = 31.048, total = 43.090 
Minus Tangent, direction = -0.727 

M000, 7.00, 14.00) to 

DO, 15.00, 14.00) 

PATH 5 

PATH NUMBER 2 Path able to extend 

cost = 18.545, est.cost = 28.438, total = 46.983 
Minus Tangent, direction = 0.257 

3-00, 18.42, 3.00) to 

MOZO, 17.37, 3.00) to 

00, 15.00, 14.00) 

PATH 6 

PATH NUMBER 3 Path able to extend 

cost = 19.049, est.cost = 28.757, total = 47.806 


Minus Tangent, direction = -0.399 
MA 00, 9.53, 3.00) to 

mm 00, 11.21, 3.00) to 

O, 15.00, 14.00) 


PATH 7 
PATH NUMBER 4 Path able to extend 
cost = 28.213, est.cost = 19.416, total = 47.629 


Minus Tangent, direction = 0.000 
2:00, 15.00, 3.00) to 

85 00, 15.00, 3.00) to 

81.00, 15.00, 3.00) to 

a 00, 15.00, 3.00) to 

(00, 15.00, 14.00) 


PATH 8 
PATH NUMBER 5 Path able to extend 
cost = 24.216, est.cost = 23.640, total = 47.856 


Minus Tangent, direction = -2.843 
829-00, 8.85, 3.00) to 

400, 10.08, 3.00) to 

(40.00, 15.00, 14.00) 


PATH 9 
PATH NUMBER 6 Path able to extend 
cost = 23.732, est.cost = 23.147, total = 46.879 


Minus Tangent, direction = 2.952 
(20700, 18.85, 3.00) to 

(24.00, 18.08, 3200) to 

(40.00, 15.00, 14.00) 

SHORTEST PATH 


(20.00, 20.00, 14.00) to 


(14.00, 20.00, 14.00) to 
(10.00, 20.00, 14.00) to 
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(1.00, 15:00, T4 009 
The total length of the shortest path is 40.911 
PATH FINDING ITERATION COUNT - 3 


PATH 1 
PATH NUMBER 0 Path cannot be extended 


cost = 10.296, est.cost = 30.414, total = 40.709 
Plus Tangent, direction = 0.507 

(10.00, 20.00, 14.00) to 

(1200) 215.00, 1300) 

PATH 2 

PATH NUMBER 10 Path able to extend 

cost = 28.613, est.cost = 21.541, total = 50.154 


Minus Tangent, direction = -1.138 
(20.00, 7.00, 14.00) to 

(14.00, 20.00, 14.00) to 

(10.00, 20.00, 14.00) to 

(1. 00 215.00, 143005 


PATH 3 
PATH NUMBER 8 Path able to extend 
cost - 41.059, est.cost - 0.000, total - 41.059 


Minus Tangent, direction - 0.000 
(105007 15.00 , 4:00 Co 

(24.00, 20.00, 14.00) to 

(20.00, 20.00, 14700) to 

(14.00, 20.00, 14.00) to 

(10.00, 20.00, 14.00) to 

(1-00, 15.00; 14:00) 


PATH 4 

PATH NUMBER 1 Path able to extend 

cost = 12.042, est.cost = 31.048, total = 43.090 
Minus Tangent, direction = -0.727 

(10.00, 7.00, 14:00) to 

(1:00, 15.00, 14:00) 

PATH 5 

PATH NUMBER 2 Path able to extend 

cost = 18.545, est.cost = 28.438, total = 46.983 


Minus Tangent, direction = 0.257 
(14,00, 18.42, 3.00) to 

C10 300 40172537 590 3500. to 

(1.00, 15:00, 14.00) 


PATH 6 
PATH NUMBER 3 Path able to extend 
cost = 19.049, est.cost = 28.757, total = 47.806 


Minus Tangent, direction = -0.399 
(14.007 9.537 3 00) tO 

(10.00, 11521, 3.00) to 

(1.00, 15.00, 14903 
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PATH 7 
PATH NUMBER 4 Path able to extend 
cost - 28.213, est.cost - 19.416, total - 47.629 


Minus Tangent, direction - 0.000 
00, 15.00, 3.00) to 

0:00, 15.00, 3.00) to 

00, 15.00, 3.00) to 

GNmO00, 15.00, 3.00) to 

2:05:00, 15.00, 14.00) 


PATH 8 
PATH NUMBER 5 Path able to extend 
cost = 24.216, est.cost = 23.640, total = 47.856 


Minus Tangent, direction = -2.843 
87700, 8.85, 3.00) to 

4:00, 10.08, 3.00) to 

0.00, 15.00, 14.00) 


PATH 9 
PATH NUMBER 6 Path able to extend 
cost = 23.732, est.cost = 23.147, total = 46.879 


Minus Tangent, direction = 2.952 
«0900, 18.85, 3.00) to 

1.00, 18.08, 3.00) to 

(40.00, 15.00, 14.00) 

SHORTEST PATH 


(40.00, 15.00, 14.00) to 

(24.00, 20.00, 14.00) to 

(20.00, 20.00, 14.00) to 

(2:00, 20.00, 14.00) to 

100, 20.00, 14.00) to 

DO, 15.00, 14.00) 

The total length of the shortest path is 41.059 


This final example is the listing generated by the algorithm for the second example 
for a two polyhedral world in Chapter IV. 


INITIAL PARTIAL PATHS 


PATH 1 

PATH NUMBER O Path able to extend 

cost = 7.642, est.cost = 31.072, total = 38.715 
Plus Tangent, direction = 1.166 

9700, 20.00, 7.64) to 

"DO, 13.00, 7.00) 

PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 6.738, est.cost = 31.694, total = 38.432 
Minus Tangent, direction = -1.107 

(10700, 7.00, 7.64) to 

«00, 13.00, 7.00) 

PATH 3 

PATH NUMBER 2 Path able to extend 

cost - 9.797, est.cost - 28.287, total - 38.084 


Minus Tangent, direction - 0.494 


l6l 


(14.00, 16:77, 3.00) to 
(10.00, 14.62, 3.00} to 
(7.00, 13.00, 7.00) 


PATH 4 
PATH NUMBER 3 Path able to extend 
cost = 9,594, est.cost = 28.712, total = 38.305 


Minus Tangent, direction = -0.432 
(14.00, 9.77, 3.00) 58» 

(10:00, 11262, .3:00(0 to 

(7.00, 13.00, 7.00) 


PATH 5 
PATH NUMBER 4 Path able to extend 
cost = 19.029, est.cost = 19.441, total = 38.470 


Minus Tangent, direction = 0.061 
(24.00, 14.03, 3.00) to 

(20.00, 13279, 3:00 SEO 

(14.00, 13.42, 3.00) to 

(10.00, 1318, 3:00) ES 

(7.00, 13,00, 3700 


PATH 6 
PATH NUMBER 5 Path able to extend 
cost = 24.216, est.cost = 23.640, total = 47.856 


Minus Tangent, direction = -2.843 
(20500, 8.85, 3.00JWtED 

(24.00, 10.08, 3.00) to 

(40.00, 15.00, 14.00) 


PATH 7 
PATH NUMBER 6 Path able to extend 
cost = 23.732, est.cost = 23.147, total = 46.879 


Minus Tangent, direction = 2.952 
(20-00, 18785, 53005880 

(24.00, 18.08, 3.00) to 

(40700, 15200; 14-90) 

SHORTEST PATH 


(14.00, 16.77, 3.00) to 

(10.00, 14.62, 3.00) to 

(720077 137007 7.00) 

The total length of the shortest path is 38.084 


PATH FINDING ITERATION COUNT = 2 


PATH 1 
PATH NUMBER O Path able to extend 


cost = 7.642, est.cost - 31.072, total - 38.715 
Plus Tangent, direction - 1.166 

(10.00, 20.00, 7.64) to 

(7.00, 13.00, 7.00) 

PATH 2 

PATH NUMBER 1 Path able to extend 

cost - 6.738, est.cost - 31.694, total - 38.432 
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Minus Tangent, 
(10.00, 7.00, 
G0, 13.00, 


PATH 3 
PATH NUMBER 7 
cost = 17.069, 


Plus Tangent, 
(20,00, 20.00, 
(14.00, 16.77, 
(10.00, 14.62, 
(MANDO, 13.00, 


PATH 4 
PATH NUMBER 3 
cost = 9.594, 


Minus Tangent, 
(14.00, 9.77, 
(0000, 11.62, 
"UO, 13.00, 


PATH 5 
PATH NUMBER 4 
cost = 19.029, 


Minus Tangent, 
(24.00, 14.03, 
(20500, 13.79, 
(14.00, 13.42, 
(000, 13.18, 
0, 13.00, 


PATH 6 
PATH NUMBER 5 
cost = 24.216, 


Minus Tangent, 
290.00, 8.85, 

(24.00, 10.08, 
(10:00, 15.00, 


PATH 7 
PATH NUMBER 6 
cost = 23.732, 


Minus Tangent, 
(20:00, 18.85, 
(24.00, 18.08, 
(40.00, 15.00, 
SHORTEST PATH 


(14.00, 
(10.00, 
(700, 


Uy, 
11.62, 
13.00, 


est.cost = 


direction = 


est.cost = 


direction = -1.107 


2:04). to 
7.00) 


Path able to extend 


22.284, total = 39.354 
0.494 

5.54) to 

3.00) to 


3.00) to 


7.00) 


Path able to extend 
28.712, total = 38.305 


direction = -0.432 


3.00) to 


3.00) to 


7.00) 


Path able to extend 


est.cost = 19.441, total = 38.470 


direction = 0.061 
3005. to 
300) to 
3700) tO 
3.00) to 


7.00) 


Path able to extend 


est.cost = 23.640, total = 47.856 


direction = -2.843 


3.00) to 


3.00) to 
14.00) 


Path able to extend 


est.cost = 23.147, total = 46.879 


direction = 2.952 
3.00) to 

3.00) to 

14.00) 


3.00) to 


3700) to 


p 


The total length of the shortest path is 38.305 


PATH FINDING ITERATION COUNT = 3 


PATH 1 
PATH NUMBER 0 


Path able to extend 
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cost = 7.642, est.cost = 31.072, total = 38.715 
Plus Tangent, direction = 1.166 

(10200, 20.00, 72764 ete 

(7.00; 13.00, 7.00) 

PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 6.738, est.cost = 31.694, total = 38.432 


Minus Tangent, direction = -1.107 
(10.00, 7.00, 7.64) to 
(7.00, 13.00, 7200) 


PATH 3 
PATH NUMBER 7 Path able to extend 
cost = 17.069, est.cost = 22.284, total = 39.354 


Plus Tangent, direction = 0.494 
(20.00, 20.00, 5.54) to 

(14.00; 16.77, 3:00} to 

(10.00, 14.62, 3 00) €o 

(7.00, 13.00, 77003 


PATH 4 
PATH NUMBER 8 Path able to extend 
cost = 16.673, est.cost = 23.143, total = 39.816 


Minus Tangent, direction = -0.432 
(20.00, 7.00, 5.549 to 

(14.00, 9.77, 3.00) to 

(10.00, 11.62, 3-007) to 

(7.00, 13,00; 7.00) 


PATH 5 
PATH NUMBER 4 Path able to extend 
cost - 19.029, est.cost - 19.441, total - 38.470 


Minus Tangent, direction - 0.061 
(24.00, 14.03, 3.00) to 

(205900. 13.79, 3:00) to 

(14.00, 13.42, 3.00} to 

(10200, 13.18, 3.00) to 

(7:00, 23.00, 7.00) 


PATH 6 
PATH NUMBER 5 Path able to extend 
cost = 24.216, est.cost = 23.640, total = 47.856 


Minus Tangent, direction = -2.843 
(20.00, 8.85, 3.00) to 

(24.00, 10-08, 3.00) to 

(40.00, 15.00, 14.00) 


PATH 7 
PATH NUMBER 6 Path able to extend 
cost = 23.732, est.cost = 23.147, total = 46.879 


Minus Tangent, direction = 2.952 
(20.00, 18:785, 3.00) tO 

(24.00, 18.08, 3.00) to 

(40.00, 15.00, 14.00) 
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SHORTEST PATH 

00, 7.00, 7.64) to 

a0, 13.00, 7.00) 

The total length of the shortest path is 38.432 
THE GOAL HAS BEEN REACHED 

PATH FINDING ITERATION COUNT - 5 


PATH 1 
PATH NUMBER O Path able to extend 


cost = 7.642, est.cost = 31.072, total = 38.715 
Plus Tangent, direction = 1.166 

(10.00, 20.00, 7.64) to 

WO, 13.00, 7.00) 

PATH 2 

PATH NUMBER 1 Path cannot be extended 

cost = 6.738, est.cost = 31.694, total = 38.432 


Minus Tangent, direction - -1.107 
(10.00, 7.00, 7.64) to 
m0, 13.00, 7.00) 


PATH 3 
PATH NUMBER 12 Path able to extend 
cost = 16.961, est.cost = 21.954, total = 38.915 


Minus Tangent, direction = 0.000 
000, 7.00, 9.76) to 

(14.00, 7.00, 8.48) to 

0200, 7.00, 7.64) to 

wmO0, 13.00, 7.00) 


PATH 4 
PATH NUMBER 10 Path able to extend 
cost = 25.202, est.cost - 21.048, total - 46.249 


T138 


Plus Tangent, direction 
2959500, 20.00, 9.76) to 
(14.00, 7.00, 8.48) to 
(10.00, 7.00, 7.64) to 
(7.00, 13.00, 7.00) 


PATH 5 
PATH NUMBER 7 Path able to extend 
cost = 17.069, est.cost = 22.284, total = 39.354 


Plus Tangent, direction = 0.494 
(20.00, 20.00, 5.54) to 

(43100, 16.77, 3.00) to 

a 00, 14.62, 3.00) to 

(7700, 13.00, 7.400) 


PATH 6 
PATH NUMBER 8 Path able to extend 
cost = 16.673, est.cost = 23.143, total = 39.816 


Minus Tangent, direction = -0.432 
02090577 0051252545 to 
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(14.00, 9.77, 3.00) to 
(10.00, 11.62, 3.00) to 
(7.00, 1 00, 7 009 


PATH 7 
PATH NUMBER 4 Path able to extend 
cost = 19.029, est.cost = 19.441, total = 38.470 


Minus Tangent, direction = 0.061 
(24.00, 14.03, 5.0000 

(20.00, 13279, 32000Mteo 

(14-00, 13.42, 3.00) to 

(10.00, 13.18, 3-00) to 

(7:00; 13.007 00) 


PATH 8 
PATH NUMBER 5 Path able to extend 
cost = 24.216, est.cost = 23.640, total = 47.856 


Minus Tangent, direction = -2.843 
(20:00, 8.85, 3 00'W to 

(24.00, 10.08; 3. 00) to 

(40.00, 15.00, 14.00) 


PATH 9 
PATH NUMBER 6 Path able to extend 
cost = 23.732, est.cost = 23.147, total = 46.879 


Minus Tangent, direction = 2.952 
(20.00, 18.85; 3.00) TO 

(24:500, 18°08, 3.200), to 

(40.00, 15.00, 14.00) 

SHORTEST PATH 


(24.00, 14.03, 3.00) to 

(20.00, 13.79; 3.00) to 

(14 00.013.42, 3.00 to 

(107007913. 18, 3200) sto 

(7-00; 13500, 7:005 

The total length of the shortest path is 38.470 


PATH FINDING ITERATION COUNT = 6 


PATH 1 
PATH NUMBER O Path able to extend 


cost = 7.642, est.cost = 31.072, total = 38.715 
Plus Tangent, direction = 1.166 

(10.00, 20.00, 7.64) to 

(7.00; 13:00, 7.00) 

PATH 2 

PATH NUMBER 1 Path cannot be extended 

cost = 6.738, est.cost = 31.694, total = 38.432 


Minus Tangent, direction = -1.107 
(10.00, 7.00, 7.64) to 
(7-00, 13007 700) 


PATH 3 
PATH NUMBER 12 Path able to extend 
cost = 16.961, est.cost = 21.954, total = 38.915 
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Minus Tangent, direction = 0.000 
ODO, 7.00, 9.76) to 

(14.00, 7.00, 8.48) to 

(10:00, 7.00, 7.64) to 

8509, 13.00, 7.00) 


PATH 4 
PATH NUMBER 10 Path able to extend 
cost = 25.202, est.cost = 21.048, total = 46.249 


Plus Tangent, direction = 1.138 
0000, 20.00, 9.76) to 

12900, 7.00, 8.48) to 

03100, 7.00, 7.64) to 

00, 13.00, 7.00) 


PATH 5 
PATH NUMBER 7 Path able to extend 
cost = 17.069, est.cost = 22.284, total = 39.354 


Plus Tangent, direction = 0.494 
(0.00, 20.00, 5.54) to 

m4, 00, 16.77, 3.00) to 

D 007 14.62, 3.00) to 

0, 13.00, 7.00) 


PATH 6 
PATH NUMBER 8 Path able to extend 
cost - 16.673, est.cost - 23.143, total - 39.816 


Minus Tangent, direction - -0.432 
80-00, 7.00,75.54) to 

m4.00, 9.77, 3.00) to 

00007 11.62, 3.00) to 

00, 13.00, 7.00) 


PATH 7 
PATH NUMBER 4 Path able to extend 
cost - 38.470, est.cost - 0.000, total - 38.470 


Minus Tangent, direction = 0.061 
(40.00, 15.00, 14.00) to 

(40.00, 15.00, 14.00) to 

(24.00, 14.03 3.00) to 

00, 13.79,13,00) to 

(14.00, 13.42, 3.00) to 

Go 0073.18, 37007 to 
(7.00713 700, z700) 


PATH 8 
PATH NUMBER 5 Path able to extend 
cost = 24.216, est.cost = 23.640, total = 47.856 


Minus Tangent, direction = -2.843 
0700, 8.85, 3.00) to 

(24.00, 10.08, 3.00) to 

(40.00, 15.00, 14.00) 


PATH 9 
PATH NUMBER 6 Path able to extend 
cost = 23.732, est.cost = 23.147, total = 46.879 
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Minus Tangent, direction = 2.952 
(20:00, 18.85, 3900 ED 

(24.00, 18.08, 3.00) to 

(40.00, 15.00, 14.00) 

SHORTEST PATH 


(40.00, 15.00, 14.00) to 

(40.00, 15.00, 14.00) to 

(24.00, 14.03, 3.00) to 

(20.00, 13.79, 3 OB EG 

(14.00, 13.42, 3:00 Ua 

(10.00, 13.18, 3.00 WEo 

(7:00, 13.00, 7.00 

The total length of the shortest path is 38.470 
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